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ABSTRACT 


Existing kinematics models for humans cannot simulate movement beyond 
geometric constraints. On the other hand, complex dynamics models are computationally 
expensive for real time computer graphics applications in Virtual Environments(VE). To 
be able to create a more realistic, real time, and computationally efficient human model, a 
simple dynamic model needs to be developed. 

The approach taken in this thesis was to develop a single rigid body dynamic human 
model with massless legs. Instead of a Lagrangian model, which complicates the 
calculations exponentially as the complexity of the system increases, the Newton-Euler 
method was chosen to derive system differential equations. Linear state feedback was used 
for postural control. As part of this research, a previous realistic looking human model is 
further developed. 

The major conclusion of this thesis is that a single rigid body dynamic model can 
be used for simulation of postural control. The simulation results contained in this thesis 
show that such a modeling technique could be used to cause a detailed kinematic 
representation of a human figure to move in a smooth and realistic way without resorting 


to complexity of a multi-link dynamic model. 
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I. INTRODUCTION 


A. MOTIVATION 


There is an increasing demand for realistic virtual environments (VE). One of the 
main branches of research in VE is human motion simulation. Kinematics and dynamics 
are the two disciplines which are used to create physically based mathematical models for 
human motion simulation. Kinematic models are quite simple to implement and 
computationally inexpensive in comparison to dynamic models. However, they are limited 
to simulation of the geometric constraints of the body parts of the human. On the other 
hand, dynamic models introduce additional physical properties of the body parts to the 
simulation, such as mass and moment of inertia, which provides more realism. It is possible 
to simulate human motion realistically with detailed dynamic models. However, the cost of 
this realism is a high degree of computational complexity. When more detailed models are 
chosen, the response time of the simulated human model increases. On the other hand, 
reducing latency to a minimum amount of time is a important requirement for virtual 
environments in which humans can interact. This places limits on the complexity of the 


model which can be used in a particular situation. 


B. GOALS 

The purpose of this thesis is to develop a single rigid body dynamic human model 
with massless legs to illustrate that such a model can simulate human motion more 
smoothly and more realistically than kinematic models, and without resorting to the 
complexity of multi-link dynamic models. The Newton-Euler method is chosen instead of 
the Lagrangian method, since the complexity of the latter grows exponentially when the 


degrees of freedom increase. 


C. ORGANIZATION 


Chapter II of this thesis provides background information and reviews previous 
work relating to the area of kinematic and dynamic methods to simulate human motion. In 
this chapter, articulated rigid body dynamics, postural control, and control of stepping are 
also investigated in addition to single mgid body dynamics. Chapter III provides an 
overview of the problem statement for this thesis and discusses mathematical modeling of 
postural control of a single rigid body with an attached massless leg. Chapter IV introduces 
the developed kinematic and dynamic computer models. Chapter V presents results and 
conclusions about the work completed. The last chapter, Chapter VI, discusses 


recommendations for future enhancements and further research. 


Il. SURVEY OF PREVIOUS WORK 


A. INTRODUCTION 

Mathematical models which define human motion in detail are complex and quite 
nonlinear. This provides a motivation to start with simple models at a high level of 
abstraction. Models which ignore the dynamic properties of body parts are called kinematic 
models. There are also models which consider only the torso to have dynamic properties. 
More complex models introduce mass and moment of inertia for more than one body part. 
The more dynamic properties are added, the higher becomes the complexity and non- 


linearity of the resulting model. 


B. KINEMATIC MODELS 

Kinematics 1s concerned with motion without considering the forces and torques 
that cause it. It establishes relations between position, velocity, acceleration, and higher 
order derivatives of position variables. 


The human body can be thought as a set of connected body parts (links). Each body 
part (link) has four link parameters to be defined: link length, (@), link twist (Q), link 


offset(d) and, joint angle(@). There exists a coordinate frame attached to each link 
[(CRAI89}. 

Manipulator kinematics investigates the transformation from frame to frame as the 
body articulates. Forward kinematics show how to compute the position and orientation of 
the end-effector according to link parameters. On the other hand, inverse kinematics solves 
for link parameters when the position and orientation of the end-effector is given. 

For the human body, it can be assumed that the joint angle is the only variable link 
parameter. Forward kinematics can be used to solve for the position and orientation of each 


body part as joint angles are given. However, this method is difficult for the animator. The 


position and orientation of each body part can also be computed with the position and 
orientation of the end-effector as an input by using inverse kinematics. Even though the 
second method introduces more complexity, in this approach the animator is able to define 


the path for the end-effector. 


C. SINGLE RIGID BODY DYNAMICS 
An advanced approach to human motion simulation introduces dynamics. There are 
three most often used methods: free body method with hard constraints, free body method 


with soft constraints, and generalized coordinates with Lagrangian[MCGH79]. 


1. Free Body Method with Hard Constraints 


This method looks at each body part as a separate free element, under the influences 
of joint torques, gravity, and reaction forces and moments. The simplest possible dynamical 
model for human motion is an inverted pendulum which idealizes the entire body as a 


single rigid link with upright posture maintained by ankle torque. 


uf 





Figure 1: Inverted Pendulum 


In human motion, joint torques are created by muscular contraction. The origin of 
reaction forces and moments is forces from other limbs or the ground. The force and torque 


equations for the inverted pendulum are [MCGH79] 


mx = F (eq. 2.1) 
ny = F,—mg (eq. 2.2) 
19 = FtsinO—F lcos® +M (eq. 2.3) 


where / is the distance from the coordinate origin to the center of gravity, J is moment of 
inertia about the center of gravity, F’ oe Ff y are ground reaction forces, and Mis the joint 


control torque which is computed by some control law 
Vie UX 1). (eq. 2.4) 
In this equation, X is the system state vector and f 1s time. 

After expressing reaction forces and moments, constraint equations are derived 
according to the geometry of the system, and differentiated twice. Constraint equations for 
this example are 

=) 1 COSO (eq. 2.5) 
and 

y = lsin® (eq. 2.6) 


with second derivatives 


ot: 
I] 


: wD 
— 9/sin8 —9 Icos® (eq. 2.7) 


and 


<2 
8/cos8-—98 ZJsin® (eq. 2.8) 


= 
lI 


The five equations above can be solved numerically or analytically to determine the 


unknowns: X, ¥, 0, F 2 F i The matrix form of the equations is 


m0 9 yee . 
Om OO = Oe a iaiie mig 
OO if —Isin@ Icos@|| 8 | = = (eq. 2) 
i 0 Isin@ O 0 llr -~8 Icos@ 
= Ay! 
0 1-Ilcos@ O O Fy Va Tne 


The behavior of the pendulum can also be expressed with a 3x3 matrix 


multiplication by analytically eliminating x and y. The 3x3 matrix equation is 


[MCGH79b] 


I —/sin®8 I/cos®@ 0 Ma 


a2 
mlsin® l 0 Fil = —m8 Icos® (eq. 2.10) 
= a2 
noe © Py I- m8 /sin8 + mg 


Since this system has only a single degree of freedom, a suitable state vector 1s 


me (OVS (eq. 2.11) 
(3) (:,) 7] 


SO 


be. 
lI 


° x 

a = ‘ (eq. Zanes 
) X5 

For simulating this model, numerical integration can be used to compute x(t) for 


any given x(0). The value of @ (or X ) is calculated by the system dynamic equations. It 


has been found in the work of this thesis that the 3x3 matrix solution is approximately two 


times faster than the 5x5 one. 


2. Free Body Method with Soft Constraints 


A more realistic approach is to consider the connection between limb segments as 
not rigid. In the soft constraint method, the constraint equations are replaced with functions 
defined by joint variables and their derivatives, which are used to compute constraint forces 
with some gain values. This increases the order of state equation by a factor of three in the 
planar case, and by a factor of six in the three dimensional case, but avoids matrix 
inversion. 

The previous inverted pendulum model becomes as shown in Figure 2 for the soft 


constraint method. 





Figure 2: Inverted Pendulum with Soft Constrains 


Instead of Eq. 2.5 and Eq. 2.6, the soft constraint equations are[MCGH79] 
= x, +lcos® (ede2.15) 


y= y, + fsin@ (eq. 2.14) 


with the corresponding soft constraint forces 


F 


Xx 


—k x, —k x, (eq. 2.15) 


The state vector for the numerical solution is 


; ee 
ee (9, 0, X p> X p> yb? yp) (eq. 267 


After obtaining the constraint forces by using the state vector values, Eq 2.1, Eq 2.2, and 
Eq 2.3 can be solved directly for translational and angular accelerations instead of requiring 


matrix inversion. 


3. Generalized Coordinates with Lagrangian 


Another way to derive the system equations is to use the Lagrangian method. This 
method does not compute constraint forces and moments, but instead uses the difference of 


kinetic and potential energies expressed in terms of generalized coordinates. 

The virtual work function, for any generalized coordinate, 0, is 

OW = Q,00 (eq. 2.18) 
The Lagrangian function is 

1 = heave (eq. 2.19) 
where K is the kinetic energy function and V is the potential energy function. The 


differential equations of the system are obtained from [MCGH79b] 


d Games 
———_-=— = Q (eq. 2.20) 


where q, represents the generalized coordinate for each 7 and O a, is the coefficient of 
t 


Og ; in the virtual work function. 


Considering the hard constraint example, Figure 1, the virtual work function is 
OW = Mode (eq. 2.21) 
where 8 is the generalized coordinate. The kinetic energy of this system is 
| apa” @ 2 
= 5 (mx +my +10 ) (eq. 2.22) 
or 
| eae ee ae. 
_— 5 (ml 8 sin0+ml 0 cos0+/0 ) (eq. 2.23) 
which simplifies to 
l 9 a: 
T = 5(ml" +10 (eq. 2.24) 
The potential energy of the system 1s 


V = mglsin® (eqne25) 


Thus, the Lagrangian function 1s 
eee a2 . 
= 5 (mi +I)0@ —meglsin® (eq. 2.26) 
By evaluation the Lagrangian derivatives, the result is that the angular acceleration of the 
pendulum is given by 


_ M-meglcos® 


i 
Meinl 


(eq. 2.27) 


For a numerical solution, the state vector 1s 


Xe 0 = a (eq: 22S) 
S) X+ 


The Lagrangian approach is perhaps the most effective one for simple cases. 
However, with an increase in the number of degrees of freedom of the system the 


complexity of the calculations grows exponentially. 


D. ARTICULATED RIGID BODY DYNAMICS 

It is certainly impossible to model the human body with complete realism as a single 
mass rigid body. One can, however, abstract it as a system of rigid bodies connected 
together with rotary joints with control torques acting at each joint. If the motion of each 
limb segment is given, and the forces and torques are to be computed, this problem is called 
the inverse dynamics problem. If the torques are given and the accelerations are to be 
determined, this is called the direct dynamics problem [KOOZ83]. 

There is an algorithm, called Articulated Body(AB), for direct dynamics which 
contains three O( ”) recursions [MCMI95]. In the first step of this method, which is 


Forward Kinematics, velocity and velocity dependent terms are computed from the base to 


the tip of each serial chain of links. The orientation of Is coordinate system with respect 


to i — 1 ’s specified by the rotation matrix [CRAI89], R j — 1 can be determined by using 


joint position, qj. which is an element of state vector. The position of 1’s coordinate 


ee ; 1—| : 
system origin according to 1 — 1, p;» 1S a constant. Then the spatial transformation 


matrix is [CRAI89] 


Xx, = | (eq. 2.29) 
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The spatial transformation matrices, along with q ;are used to compute velocity 
values of the links by using the relation 
= nen Vey (Eqn 2.30) 
The velocity-dependent bias forces, B.. and the vector of Coriolis and centripetal 


accelerations, C; , of each link are also determined from the base to the tip. Details of this 
calculation can be found in [MCMI95b]. 


In the second step, which is backwards dynamics, Jt ; and Bt ; are computed from 
the tip to the base. The matrix [ ; 1s the 6X6 inertia of links 7 though N. In other words, 


“the inertia ‘felt’ at the 1 coordinate system when the joints from 1 + 1 to WN are free to 
move’ [MCMI95]. The vector Bi ; 1s the bias force exerted on 1th link, including all 


outboard torques. 


In the final step, joint and link accelerations are computed from the base to the tip 


as dy = 0 is given and by using the equations 

l : . 
a, = X,_,4;_,+0,4,4+ 5, (eq. 2.31) 
and 


f, = Uja,- Bi; (eq. 2.32) 
where f ;1s the spatial force exerted onto link 1 by its inboard link which contains the effect 


of input torque, 7’,, and where a’ is a six element vector containing the angular 


1] 


acceleration, O;, and translational acceleration vectors. Each OF iS a six element unit 


vector which specifies the corresponding joint axis. 
Another numerical approach to the recursive direct dynamics is based on solving 
the inverse dynamics problem for a sequence of n + 1 specialized acceleration vectors. In 


general, the inverse dynamic equation can be written as [KOOZ83] 
T = C0-D (eq. 2.33) 
where the matrix C is given by 
—] 
ia — ass sale a [C,C,C3...] (eq. 2.34) 


and where each element of vector C is a column vector. Usually, C is not known 


explicitly. However it can be computed with a numerical approach using an inverse 


: T 

dynamics model such as that in [CRAI89]. Specifically, for the case 9 = (0,0,...) , 
from Eq 2.33 

T, = —D (eq. 2.35) 


The torque Lo can be computed by using the kinematic and dynamic equations of the 


articulated mechanism. As an example, the model given in [KOOZ83] which is illustrated 
in Figure 3 can be used. 


The kinematic equations of the planar model in Figure 3 are 


— 2 
x, = Xs (ie oe 1 (0; - , sinO,_ | ns O;_,cosO,_ ) 


: ae 
—d (0;sind, + 0; cos 0.) (eq: 223) 


and 


Deen 25) (8;_ 1 cos8,_ ,-0;_;sm0,_,) 


Ke a2 
+ d:(8;Cos8 —8; cos6,) (eq: 2:37) 





Figure 3: Free-body for Typical Link of Serial Open-chain Planar Articulated Mechanism 
[KOOZ83] 


Eq. 2.36 and Eq. 2.37 are used to calculate the translational accelerations of the 
links from the base to the tip. By using these accelerations, joint torques can be computed 


in the same recursive manner, but from out to inward with the following equations: 


Py = Pe +m,x, (eq. 2.38) 
any = hy + mg (eq. 2.39) 
r,=T,;, Fy (Uj —dj)sinO;+ Fy (1; —d;) cose; (eq. 2.40) 
-F,d;sin®; + F, d;cos®; + J 8; (eq. 2.41) 


: Vf - 

For the case® = (1,0,...) , all accelerations except 8; would be equal to zero 
which means 

r,=cC,-D (eq. 2.42) 
The joint torque, r,, can be calculated with the recursive method, which is described 
above. Then C ) can be calculated as 

Cia |) +e (eq. 2.43) 

With the same logic C;, is 

ai iF (eq. 2.44) 
Thus, the C matrix can be computed numerically by following these steps n times for a n- 


link system. Then Q is given by 


3 = Chair To) (eq. 2.45) 


where J is any arbitrary torque and [9 is the “equilibrium”’ torque defined by Eq 2.35. 


3 
Because of the required matrix inversion, the second method has O(n) complexity. It has 


3 
been shown [MCMI94], that O(m ) methods are best for simple serial chains of rigid 


bodies when n < 3, and O(n) methods are better for n > 3. 


E. POSTURAL CONTROL 
It is clearly necessary to begin with modeling stable standing of a human before 
considering control of walking. All the methods derived in the previous sections show how 


to compute joint angle accelerations according to joint control torques. The question is how 


to formulate control torques. One straightforward way is linear state feedback control 
[MCGH86]. 


A suitable state vector for postural control can be defined as 
Me= |) 2%; (eq. 2.46) 


where 9 is an nx1 vector of joint positions and Q is the nx1 vector of joint velocities. The 
corresponding body state equation is[KOOZ83] 
¢ = f(x) +E(x) T (eq. 2.47) 
In Eq. 2.10 f and, E& are in general not available analytically. However, with the 


numerical approach, explained in the previous section, the same equation can be expressed 


as 


0 
io Ol x + a 5) (eq. 2.48) 
00 ea 

where / is the unit matrix. 


Linear feedback systems permit much flexibility to the designer, such as pole 


=| . 
assignment [KOOZ83]. Since C and L'9 are functions of x, the system is not linear. 
However, it can be linearized around 0 for joint angular rates and 90 degrees for joint angles 


for erect body position. Then the linearized body state equation can be written as 
PN (sr) (eq. 2.49) 


where F' is the linearized system matrix and G is the control distribution matrix 


[KOOZ83]. 


Under the assumption of linear state feedback, the control torques vector will be 
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PT=kKx (eq. 2.50) 


where K is the gain matrix. If Eq. 2.12 and Eq. 2.13 are combined, the derivative of the 


state vector 1s then 
X= Fx+OKX =F + Grae —eax (eq. 2.51) 
Gain values in K should be chosen so that all eigenvalues of H have negative real parts in 


order to obtain a stable postural control [KOOZ83] [CAMA77]. 


F. CONTROL OF STEPPING 

If all the postural control system eigenvalues have negative real parts, the 
articulated mechanism can maintain its upright standing posture. The next step is to add 
stepping to the system. At this point there are two options to choose: static or dynamic 
balancing. “A statically balanced system avoids tipping and ensuing horizontal 
accelerations by keeping the center of mass of the body over the polygon of the support, 
formed by the feet.” [RABI86]. On the other hand, a dynamically balanced system can 
depart from static equilibrium and is permitted to tip and accelerate for short period of time. 
By observing human walking, one can easily say that dynamical balancing needs to be 
chosen for simulating a stepping human. 

A step length control method with dynamic stability was presented by Gubina 
[GUBI74]. Gubina’s model has a single rigid body with two supporting massless legs. The 


system has three degrees of freedom. The state vector for the linearized system is 
| ee: 
a — Cee i E07, Gig: 85) (eq. 2.52) 
where 0, is the leg angle, 0. is the body attitude, r is the leg length, and rp 1s the 


desired leg length. 





Figure 4: Single Rigid Body Model with Variable Length Massless Supporting Legs 
[GUBI74] 


The input vector for the linear state equations is 


T 
i (u,, U- ) (eq. 2.53) 
where 
and 


In these equations, F' is the control force applied along the leg, M is the control torque 
applied at the hip and F 0 M 0 are bias force and torque respectively. 


The input vector, U4, is used to control leg length, and body attitude and step control 


are used to produce the desired forward motion. The leg length is controlled by U, as 


uy, =h (r-ro) + hot (eq. 2:56) 


where A, and /, are gain constants. The body attitude is controlled by Uy as 


where ht 5 and he are gain constants and © is the bias term to permit the desired attitude. 


When the initial conditions are assumed as r(Q) = ae r(Q) =e 


8,(0) = 0, 85(0) = 0, the leg angle is governed by the linearized system equation 


0, -b°0, = 0 (eq. 2.58) 
where 
Ze § 
b* = Ce (eq. 2.59) 





Figure 5: Stable Gait Function [GUBI74] 


As seen in Figure 5, the leg has a negative angular velocity growth rate for negative 
angles and positive angular velocity growth rate for positive angles. The horizontal dotted 


line which 1s extended from the right to the left represents the change of supporting leg. In 
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the absence of disturbances, the leg angle can be described with a periodic function, 0 (ft). 


The existence of such a function provides a “‘stable gait”. However, disturbances do occur 
in locomotion systems. That is why feedback control is needed. With such a control 


mechanism, the leg angle differential equation becomes 


, 0 1 1 
X= x.—| Ju.(k)o(t-—kT) (eq. 2.60) 
D S S 
b” 0 0 
where 6 denotes the unit inpulse function and 


T 
x = (X3, x4) (eq. 2.61) 


It is shown in [GUBI74] that a suitable discrete time control vector for this system is 
40 
ag) OVP) x (kr) -— | +0, (eq. 2.62) 


The angle, 9 & is the desired total leg angle excursion over one cycle, while Vo is the 


desired forward speed. 

Another foot placement algorithm for controlling forward speed is investigated by 
Raibert[RABI74]. In this work, a one legged hopping machine was developed to 
investigate dynamically balanced running robots. The machine has two main parts: the 
body and the leg which is connected to the body with a hinge. There also exists a hip 


actuator to be able to apply a torque from the body to the leg. 
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Figure 6: One Legged Hopping Machine with Control Variables [RAIB86] 


Neutral Point 


ee ES 
Figure 7: Placing The Foot at The Neutral Point [RABI74] 


For each forward speed there is a foot position which causes zero acceleration in the 
direction of motion. This point is called the “neutral point”. Placing the foot behind the 
neutral point accelerates the machine, placing the foot in front of the neutral point 


decelerates it. 
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Acceleration Deceleration 


Neutral Point Neutral Point 


Figure 8: Acceleration and Deceleration of the one Legged Hopping Machine According 
to Neutral Point[RAIB86] 


To place the foot at the neutral point, the control system should extend the leg such 


that [RAIB86] 


te = a7 (eq. 2203) 
where X fo is the forward displacement of the foot with respect to the center mass, X is 


the forward speed, and T , is the duration of the stance phase. An additional displacement 


is needed to correct the errors in the forward speed: 
Xp — k (%-X) (eq. 2.64) 
where X f is the displacement of the foot from the neutral point, x q 1s the desired 
A 


forward speed and k.,, is the feedback gain constant. 


By combining the equation Eq. 2.64 and Eq. 2.65, the total foot displacement 1s 


21 


xT. 


Then the required hip angle for the desired velocity is 


(eq. 2.66) 


iP kK (Xx — =) 
2r r 


Ya = © —arcsin( + 


where Y is the angle between the leg and the body [RAIB86]. 

Another research focused on biped systems was conducted by Troy [TROY96]. He 
investigated locomotion of dynamically balanced biped mechanisms. Different multilink 
planar and spatial biped models were developed by using feedback control for balancing 
and walking. In general, his work extends the results described above while confining 


locomotion to a specified sagittal plane. 


G. SUMMARY 

This chapter provides a survey of previous work relating to modeling and control 
of posture and gait for a walking human. It starts with kinematic modeling and continues 
with single and articulated ngid body dynamic models. One section discusses simulation 
of postural control for erect posture of the body. The last part of the chapter illustrates two 
different stepping control approaches for a given desired forward speed. The next chapter 
of this thesis derives the differential equations of a human model which has two degrees of 
freedom by using the Newton-Euler method. It also discusses postural control for the same 


model. 


I. DETAILED PROBLEM STATEMENT AND MATHEMATICAL 
FORMULATION 


A. INTRODUCTION 

The model in [GUBI74], Figure 4, 1s designed to simulate postural and gait control 
with three degrees of freedom. The body attitude and the ankle angle are used for postural 
control and the variable leg length and foot placement as well as periodic stepping are used 
for gait control. The model has one input control torque and one input control force. 
Another similar model, dealing only with postural control, will be introduced in this 
chapter. This model has two degrees of freedom with the same variable angles of the 
previous model. Instead of the input control force, this model, shown in Figure 9, has a 


second input contro] torque at the ankle. 


B. DESCRIPTION OF THE MODEL 


ib 


TST 
/ 


M, 


Figure 9: Single Rigid Body Model with A Constant Length Massless Supporting Leg 


This model, Figure 9, has a single rigid body with one supporting massless leg. The 


system has two degrees of freedom. The state vector for the system is 
er ob 
SG 8, aes) (eq. 3.1) 


where 0, is the leg angle and 0, is the body attitude. The leg length, 7 , is constant. 


The input vector for the state equations is 


iia (Uy, U5) (eq. 3.2) 
where 
u,=M, | (eq. 3.3) 
and 
uy = M, (eq. 3.4) 


In these equations, M | is the control torque applied at the ankle and M., is the control 


torque applied at the hip. 


The input vector, 4, is used to contro] leg angle and body attitude. According to 


general linear feedback control 


M | 
a= (u,) = K(x-X9) (eq. 3.5) 


Where X¢ 1s the desired state vector. A special form of this equation, called local feedback, 


has been proposed for postural control [KOOZ83][CAMA77]. In local feedback, each joint 


torque is determined by angle and rotation rate of that joint only. This decouples control of 
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joints from each other. Specifically, in this form of control, the leg angle is controlled by 


Uy as 
u, = kg (8, - a) +k, (eq. 3.6) 


where Ko and &, are gain constants and ©, is the desired leg angle. The body 
or = 


attitude 1s controlled by U> as 


where Kg, and k g, are gain constants and , is the bias term to permit the desired 
2 
body attitude. Local contro] will be examined further in this thesis. 


C. THE LAGRANGIAN VERSION OF THE PROBLEM 
The components of the linear velocity vector of the center of mass of the single rigid 


body can be derived from angular variables as follows: 


y = r6,cos0, +/65cos8, (eq. 3.8) 


-r@, sin®, 165 sin, (eq. 3.9) 


N. 
I 


The kinetic energy of the system 1s thus 


; 2 2 
K = =(r0 cos®, + 16080) + 5m(-r6; sin 0-16) sin®,) 


aU 
+ 516) (eq. 3.10) 


The potential energy of the system can be expressed as 


V = mg(rcosO, + /cos0,) (eq. 3.11) 


Thus, the Lagrangian function becomes 
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] 2 a 
j ge 5/n(7r8) COsO, + 10 cos 0, ) + 5m(-r6} sin@ 165 sind 


| 
+ 5109 —mg(rcos@, + /cos0, ) 
The differential equation for the first generalized coordinate, 0, iS 


i 
dt96, 08, 





= M,-M, 


Gil 
The term, ———, in equation Eq. 3.12 can be derived as 
] 


— = mr’, (cos0, 2+ Imr®5cos0, cos0, 


] 
+ mr 6,(sinO,)? + /mr8ysin@sin8, 
Then, the first term in the equation Eq. 3.12 is 


d oL 
dt9@, 





“ ee 
= mr’6(cos@,)2 - 2mr~;cos®, sin®, 


+Imr®@> cos 9 | cos 6,—Imr850 | sin0, cos0,, 


2 : 
—Imr 07 cos, sinO,, + mr, (sin) 


Dee nl ; 
+2mr 9;sin@, cos, + mirO7sinO, sin8, 


- im 
+ mlr879, cosO, sin8, + mirO>sin8 , cos, 


And the second term in the equation Eq. 3.12 1s 
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2 
2) 


(eq. 3.12) 


(eq? sala} 


(eq. 3.14) 


(eq. Sale 


Ot = _mr?6} sin® , cos8, - mri6 65: 
50, —mr 9;sin0, cos@, —mrl@85sin@ , cos6,, 


Do, ; Mes ; 
+mr 8; cos@,sin8, + mrl6,8,cosO, sin®, 


+mgrsinO, (eq. 3.16) 


By substituting Eq. 3.14 and Eq. 3.15 into Eq. 3.12, the following equation can be obtained: 


mr’ 6, + Imr65(sin@ , sin®, + cos®, cos®,) 


ae 
+ Jmr@>(sin@, cos@,, — cos, sin 0.) 


+ Imr858;(cos@, sin8, — sinO, cos6,) 
+Imr@, 05(sin®, cos 0, — cosO, sin8,,) 


—mgrsin@ | = M,-M, (eq. 3.17) 


After applying trigonometric conversion rules, the first differential equation of the model 


can be derived as follows: 
De % oe 
mr 8, +lmr0.cos(0., —86,) + /mr®7sin(8, — 9.) 
—mgrsin®, = M, -M, (eq. 3.18) 


The differential equation for the second generalized coordinate, 0,., 1S 


fg 


doL OL 
digg, 00,2 


The term, ——, equation in Eq. 3.19 can be derived as 
) 
2; 2 
he = mlr0,cos0, cos0, + ml 02( cos, ) 
965 


ae : 2. : y 
+ mirQ@;sin0,sinO, + ml 0o(sin0,) +105 
The time derivative of Eq. 3.20 1s 


doL 
dt9@, 


ss 2 
= mir, cos, cos0, — mir0;sinO, cos0., 





— mlr®@,0,cos0, sin0, + ml” 65 (cos @,)° 
ee? ; ae ; 

—2ml 85 cos@, sin@, + mir8;sinO, sind, 
e ——— 

+ mir0;cos0, sinO, + m/r0)9sin6, cos8., 


Dex . 2 26 a re 
+ ml 99(sin8,) + 2ml*87 sin. cos0, + [8 
Then the second term in the equation Eq. 3.18 can be derived as 


= —- mlr®,9cos0, sinO,, + mlr®,85sin8, cos0., 


+ mglsin®O, 


(eq. 371) 


(eq. 3.20) 


(eq. 3.21) 


(eq. 322) 


By substituting Eq. 3.21 and Eq. 3.22 into Eq. 3.19, the following equation can be obtained: 


. i's. 
mlr®) cos® , cos0., — mir8) sin, cos, 


~ mlr@ 65c0s8, sin®, + mi” 65(cos0,)” 
Di! . a ; 
—2ml 8 cos0,sin8, + mir8;sin@ sind, 
wo) es 
+ mirQ, cos, sinO, + mlr8;67sin8, cosO,, 
Des ; 2 2A , i 
+ ml Q5(sin8,) + 2ml*87sin0, cos6, + 16> 
+ mlr0,8> cos 0, sin@,—mir0 | 6sin0, cos6, 
+ mglsin0, = M, (Se Sey. 


Then, the second differential equation of the model can thus be expressed as follows: 


us sl, 
mlrQ,cos(8, —8,) + mir@,sin(@,—9,) 


+ (J + ml” )by—-mglsin®, == M, (eq. 3.24) 


The model in [GUBI74] has an additional degree of freedom comparison to the 
model presented in this thesis which is variable leg length and does not have any ankle 
control torque. If the time derivative values of the leg length variable 7 are taken out, and 


the ankle torque M 1 is added to the dynamic equations in [GUBI74], it is seen that these 


equations are identical with the equations Eq. 3.17 and Eq. 3.23. 


pA) 


D. NEWTON-EULER FORMULATION 

As discussed in the previous chapter, if the Lagrangian method is used, an increase 
in the number of degrees of freedom complicates the calculations exponentially for an 
articulated mechanism. Even though the number of variables are not many in this case, by 
considering that future research 1s likely to investigate more complex models, the Newton- 
Euler method looks as if a better approach for this kind of problems. 

The model, presented in this chapter, can be divided into two parts to simplify the 


problem. These parts are the massless leg and the body itself. 


1. The Newton-Euler Equations of the Massless Leg 





Figure 10: Free Body Diagram for The Massless Leg 


Because the Jeg has no mass, it is only possible to talk about the static equilibrium 


of this body part. The equilibrium equations can be expressed as 
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and 


where M ., F’. andr are defined in Figure 10. 
Equation Eq. 3.25 can also be written as 
M,-M.,-rF, = 0 

so the reaction force, F - is given by 


M,-M 
je 


r iF 


2. The Newton-Euler Equations of the Body 





Figure 11: Free Body Diagram of The Body 


3] 


(eqro225)) 


(eq-5.26) 


(Eqmo.27) 


(Cd... 6) 


The dynamic equations of the free body in the z and y directions can be derived as 

mz = —mg+Fcos0, —-F sin, (eq. 3.29) 
and 

my = Fsin0, —-F.cos0, (eq. 3.30) 


For the angular motion of the free body, the dynamic equation of the body can be expressed 


as 

10. = M,—IF .cos(9, —9,) +/F sin(9, —9,) (eq. 3.31) 

3. Combining Body and Leg Equations 

As shown in Figure 9, the geometric constraint equations for the model can be 
derived as 

= rsinO, + isin@.,, (eq. 3.32) 
and 

ae rcosO, + icosO.. (eq. 3.33) 


The second time derivatives of Eq. 3.30 and Eq. 3.31 are as follows: 


e ae - a2 
= r0;cos0, —r0;sinO, + /05 cosO, — /02sin0., (eq. 3.34) 


— 


and 
: D ae 2 
Z = —r0,sinO, —r8;cosO, —/6sin8., — 18. cos9, . (eq. 33) 


The constraint force F’ , in equation Eq. 3.29 can be eliminated by using equation Eq. 3.26 


resulting in the expression 


i((M,—-M.,)cos(@, -6,) 


cy = M., 
r 


— 


+ /Fsin(0, —6,) (eq. 3.36) 
By combining Eq. 3.28 and 3.32, the following equation can be derived: 
m(rQ;cos@, —rQ;sinO, + /85cos0, —/0sin8,) = 
= Fsin0, + F .cos@, (eq. 3.37) 
On the other hand, after substituting Z with Eq. 3.33, Eq. 3.27 can be written as 
- 2 ee e. 
m(—r0,sin0, —r0; cos, —/05sin0, —/65cos8,) = 
= —mg + Fcos0, —-F,sin0, (eq. 3.38) 


The equation Eq. 3.34 can redefined as 


; M,(1+r)-IM, 
691 - Flsin(8,- 8) = cos, - 8) (eq. 3.39) 


After reorganizing, Eq. 3.35 and Eq. 3.36 become 


6,mrcos®, = 65mIcos8, —Fsin0, = 


Ae eae M, 9 
= mr®, sin0, + ml85sin8,, + — a") (eq. 3.40) 
and 
O,mrsinO, + O2m/sinO., + Fcos0, = 


© 2 on 
= —mr®,cos@ ,—ml84cos0., + mg + ———sin8, (eq. 3.41) 


Equations Eq. 3.37, Eq. 3.38, and Eq. 3.39 can be expressed in matrix form as: 
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0 I —/sin (9. — 9, ) 6, 
mr cos® mlcos0., —sin8, 6, = 


mrsinQ, misin@., cos0, re 


| —_ = 


M,(l+r)—IM, 
— COSI = GF) 


M,-M 


a, = 
mr®, sinQ, + mlQ>sin0., + —_= eee (eq. 3.42) 
r 


6; cos0, — ml0>cos0 — 
—mr9,cos0, —ml0jcos8, + mg + ——— sin; 

An alternative to this analytic formulation of the Newton-Euler formulation is to 
use the recursive approach explained in Chapter II of this thesis. Thus results in more 
complex logic, but in less computation, especially as the number of links in the dynamic 


model increases. 


E. LINEARIZED ANALYSIS FOR CHOOSING GAINS 

As mentioned in the previous sections, this system has two inputs, hip and ankle 
torques. The main purpose of postural control is to determine these input torques in order 
to maintain the upright orientation of the body. The approach taken in this thesis is linear 
state feedback control. The control equations, Eq. 3.5 and Eq. 3.6, are presented in the 
second section of this chapter. For the local control model, the problem is to compute the 


required gain values: ke mi 


. Kae 
6,’ 92’ 65 
It is reasonable to drop quadratic velocity components for small motion linearized 


ao ee 
analysis [MCGH86]. Under this assumption, 0; and 05, components are removed from 
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all equations. The same assumption means that 0, and 0. can have only small values 


which allows the substitutions 


sin, = 0. 

sin0. = 0., 

cos0, = 1, 
and 

cos0, = 1. 


With these considerations, equation Eq. 3.18 can be rewritten as 
oe : 
mr 9, +ImrOy = mgr0,+M,-M,. 
The linearized version of equation Eq. 3.24 is 
: Ds: 
mlirQ, +([+ ml )85 = mgl0,. + M, 
05 can be evaluated from equation Eq. 3.47 as 


mgr0,+M,-M, _mr6, 


6, = 
2 [mr 


(eq. 3.43) 


(eq. 3.44) 


(eq. 3.45) 


(eq. 3.46) 


(eq. 3.47) 


(eq. 3.48) 


(eq. 3.49) 


If equation Eq. 3.49 is substituted in equation Eq. 3.48, the following equation is obtained 


(mir) 6, + (I+ ml”)(mgr®, + M, — M, -mr*6y) 


oe 
= rm gl 8. + mirM, 
After reorganizing, equation Eq. 3.50, 0, can be defined as 


6, = 
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(eq. 3.50) 


2 mrg0, —mgr(I + ml2)0, — (1+ ml2)M, + (Imr + 1+ ml” )M, 
—Imr 


(eq. 3.51) 


The angular acceleration, 0 , can be defined by using equation Eq. 3.47 as 


. mgr@, + M, —M,—Imr®5 
, ee 9,352) 
mr 


After substitution of F , equation Eq. 3.48 can be rewritten as 
] a Qn : 
~(mgrd, + M, -M, —Imr02)+ U+ mI )82 =mgl0, + M, (eq. 3.53) 


After reorganizing equation Eq. 3.53, 05 can be define as 


0, ae 

mglr0.—ImgrO, -1M, + (r+ 1)M, 

(Cquaes 
rl 

The general linear state feedback equation 1s 

Nei cet (Eq. oe) 


The state vector, xX, is defined by equation Eq. 3.1. If the desired values for the angles are 
chosen as zero, the input vector, in equation Eq. 3.5, can be rewritten as 
u = Kx (eq. 3.56) 


VY LO5 


After substituting “ according to the equation Eq. 3.56, Eq 3.53 can be written as 


X = (A+BK)x (eq. 3.57) 


where 


0 ie, 0 


SORE Se I’m’ re 
ee 0 


Z 
= Imr [mr ; (eq. 3.58) 


0 Peo 


—Imegr melr 
Ir : Ir : 


and 


0 0 
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B= | Imr Imr (eq. 3.59) 


Ir Ir 
For local feedback, K can be defined as follows, according to the equavions Eq. 3.2, Eq. 


3.6 and Eg 3.7 


-—K, -K. O 0 
6 
K = ; - (eq. 3.60) 


where desired angle values O, and QO. are taken as zero. Then, by using equation Eq. 


3.58, Eq. 3.59 and Eq. 3.60, A + BK can be defined as 
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(eq. 3.61) 


(eq. 3702) 


(eq. 3.63) 


(eq. 3.64) 


(eq. 307) 


(eq. 3.66) 


(eq. 3:67) 


(eq° 36a) 


(eq. 3.69) 


For a stable upright posture, all roots of the characteristic equation (eigenvalues) 
should have negative real parts [KUO95]. The characteristic equation of the closed loop 


system is [KUO95] 
IsI-(A + BK)| = 0 (eq. 3.70) 


After substituting the equation Eq. 3.60 the equation, Eq. 3.69 can be rewritten as 


s-l1 0 0 
CDEF = 0 (eqs. 71) 
0 0 s -l 
| GHW Y | 
where 
Z yi 
—mgr(l+ml as emi) 
= ey (Eqpo. 2) 
2 
Imr 
2 
Ke (I+ml ) 
py See ! —7 (eGaon) 3) 
Imr 
eo Z 
ee eat ctert. ©) 
E-= - —_——— (eq. 3.74) 
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Imr 
Z 
Ke (imr+I+ml ) 
ee (Bp 2D) 
Z 
Imr 
imgr—Kg | 
C—— (eq. 3.76) 
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se 
H = — ey 
a (eq. 3.77) 





“mgr + Kg (r +0) 
Ww = —_—__., Bh 
oT (equsiie) 
Ke (r +1) 
Y = 5+——_—_ 22. 
S T (eq. 3.79) 


If the determinant of in the equation Eq. 3.69 is evaluated, the following polynomial is 


obtained: 
se ie 
+s°(IrmK +/1K. +K Pe a 
0. 0 0; 60> 
+ s(IKy - Pare + mI Kg — rm’ Ig 
Z 
+r mK +rmiky +K, K, —imgr) 
+ SCA Ke ime k + Ka Ko —msrk, ) 
+ (me? Fa en ne aie J=0 3.80 
§ Ss = 0,6, | — (eq. 3.80) 


It is clearly a very hard problem to determine the required gain values in equation Eq. 3.80, 
which result in all roots having negative real parts. Instead, a experimental solution was 
found by trying some gain values on the computer model. It was found that the following 


values produce an upright stable body posture: 
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Ke =o O00 (eq. 3.81) 

K = 2500, (eq. 3.82) 

Kg, = 4000, (eq. 3.83) 

eaa= 400 (eq. 3.84) 
Oo 


F. SUMMARY 

In this chapter, a new human dynamic model, which is made of a body and a 
massless leg, is introduced. The dynamic differential equations of the model are derived by 
using two different methods: the Lagrangian and the Newton-Euler methods. The last 
section of the chapter discusses the small motion linearized analysis of the system. It is 
explained how the gain constants for the input torques should be chosen. 

The next chapter will explain the implementation of computer models based on the 


knowledge presented in the second and third chapters of this thesis. 
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IV. COMPUTER MODELS 


A. INTRODUCTION 

In the second chapter of this thesis, it is pointed out that there exist two major 
methods to simulate human motion simulation: kinematic and dynamic models. The 
present chapter first presents stepping algorithms for a human figure implementation which 
is developed by using a kinematic model. The second chapter also discusses the general 
approaches for dynamic simulation of human motion. Based on this knowledge, the third 
chapter investigates the mathematical representation of a dynamic human model. The 
present chapter also introduces the computer implementations of these models. 


B. KINEMATIC COMPUTER MODEL (DYNAMAN) 
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Figure 12: Kinematic Computer Model: Dynaman 
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The kinematic model presented in this section is made of fifteen separate body parts 


as shown in Figure 13. 
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Figure 13: The Body Parts and Corresponding Dynamic Coordinate Systems (DCS) of 
Dynaman 


The purpose of the simulation is to create a computer representation of a stepping 
human. Under this requirement, it is clear that the main concern is leg and foot motion. The 
other body parts are synchronized according to the legs. 

Inverse kinematics is chosen for computing leg motion. Inverse kinematic 
equations take the position of the end effector (foot) as the input and computes each joint 
angle of the leg. The points which describe the position of the foot in space for a full gait 
period produce a path. The question is to define this path as a mathematical function of 
time. This function, of course, should be developed with the knowledge of the geometry of 
the environment, such as height of each stair. 

Forward kinematics could have been chosen for the same problem. In that case, 
joint angles of the leg would be the input to the forward kinematic equations, and the 


position of the end effector would be calculated. For this second case, there should exist a 
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feedback system in order to input the constraints of the environment to the model. A 
collision detection mechanism between the foot and the floor seems to be appropriate to this 


approach [GOET94]. 


1. Inverse Kinematic Equations for Three Link Planar Manipulator 





Figure 14: Three Link Planar Manipulator 


The leg of Dynaman can be thought as a three link planar manipulator which is 
made of the upper leg, the lower leg, and the foot. The frames which are attached to the 
links are shown in Figure 14. The general form of the transformation matrix to represent a 


point in the frame 1 — | which is defined in frame I is [CRAI89] 


1— | 
l 


= 
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cos@. —sin@, 0 a, _| 
sin8.cosa. , cos@.cosa. , —singd. , —d.sind. 
= I 1— | I 1— 1 1— | l 1— 1 (eq. 4.1) 
sinQ sina, , cosO,sina;,_, cosa, , d,cosa, | 


0 v 0 1 


where @, O, O, and d are the link parameters as defined in the second chapter of this 


thesis. The link parameters for the presented model are defined in Table 1. 





Table 1. Link Parameters of the Leg [CRAI89] 


In this model, all Z, axes are parallel, and all x; axes are in the same plane. That is 
why allthe @,_ , and d, values are zero. The parameters /, and /, are the link lengths 


of the first and second links. The parameter l. defines the position of the toe point. Since 


it is in the end effector frame, it is not included in the link parameters. 
According to equation Eq. 4.1 and Table 1, the transformation matrices for the 


neighbor links are 


cos0, —sin8 | O 0 
0 2 sin0, cos0, 00 


i 
0 0 10 
| 0 0 oii 


(eq. 4.2) 
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cos0, —sin6., Ol, 


I 0 0. 
oT = sin8,. cos0, 0 0 (eq. 4.3) 
haa 
0 1| 
cos0 —sin0, 0 L, 
<T 7 sin0, cos0, 0 0 (eq. 4.4) 
0 0 Pet) 


| 0 oO) @ al 


The transformation matrix between the first and the last links can be derived by using the 


product 


T (eq. 4.5) 


If the matrix multiplications are executed after substituting equations Eq. 4.2, Eq. 4.3, and 


Eq. 4.4 into the equation Eq. 4.5, the following transformation matrix will be obtained: 


0 


Hl 


cos(0, + 0, +03) —sin(0, +9,+9,) 0 /,cos0,+/,cos(0, + 9,) 
sin(Q,+6,+6,) cos(@,+0,+9,) 0 /,sin0,+/,sin(6, + 8,) 
0 0 | 0 
\ 0 0 0 1 | 
(eq. 4.6) 
If the method in [CRAI89] is used, the transformation matrix of the end effector 


according to the base link can be defined as 
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cosO -sin® 0 x 

07 _ |sin@ cos@ 0 y 
3f = 

0 O 10 

Oo (OT 


(eq. 4.7) 


where the position components, X and y, and the orientation, 8, of the end effector 
according to the base link are known. The following equations can be derived from 


equations Eq. 4.6 and Eq. 4.7: 


cos8 = cos (0, + 6, + 83) (eq. 4.8) 
sin8 = sin(0, + 0, + 03) (eq. 4.9) 
be I, cosO, + l,cos(0, + 8.) (eq. 4.10) 
y = J,sin@, +/,sin(6, + 8.) (eq. 4.11) 


If the squares of equations Eq. 4.10 and Eq 4.11 are added, the following equation is 


obtained: 
2 2 Dig ee?) 
x ty =1,+1,4+21,1,cos8, (eq. 4.12) 


In equation Eq. 4.12, the only unknown is @,, and it can be defined as 


2 D: WE Z 
0. = acos| ———-_-__—_——_- (eq. 4.13) 
21,1, 


After having found 9. , equations Eq. 4.10 and 4.11 can be can be written as 
x = k,cos0, —k,sin0, (eq. 4.14) 


and 
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y = k,sin0, +k,cos0, (eq. 4.15) 
where 
k, = 1, + 1,cos0, (eq. 4.16) 
and 
k, = 1,sin0, (eq. 4.17) 


After following the steps in [CRAI89], 0, is defined by the equation 


} Kk, 
0, = atan(* }-atan k, (eq. 4.18) 


Then foot angle can then be calculated as 


@, = 0-0,-8, (eq. 4.19) 


2. Link Descriptions in the Computer Model 


The computer model has been developed in the IRIS Performer!!! application 
environment. This environment allows the programer to create a Dynamic Coordinate 
Systems (DCS). DCS lets the programer to change the transformation of the objects which 
are attached to that DCS node. The method followed in this application 1s to attach each 
body part to a DCS node and connect all the DCS nodes in a tree structure which creates a 
hierarchical structure. For example, if the upper leg is rotated for some degrees, all the body 
parts under that DCS node, lower leg and foot, follow this rotation. DCS tree structure of 


the whole body is shown in Figure 13 and Figure 15. 
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Figure 15: Dynamic Coordinate System (DCS) Hierarchy Tree of Dynaman 

The body parts of the Dynaman are taken from an other model developed in 
OpenGL® by Will Frey at the Naval Postgraduate School [FREY96]. However, while Frey 
used body segment Euler angles relative to an Earth fixed reference system, as described 
above, the work of this thesis is based on joint angles. The coordinates of the polygons, 
taken from the other model, which produces the body parts, are loaded to the application 


by using poly format files. 


3. Stepping Algorithms 

The inverse kinematics model takes position and orientation of the end effector and 
computes each joint angle. Then, the question is to define the path of the end effector (foot) 
which is the input to the inverse kinematics model. Two separate algorithms are developed 


to define the path: stepping forward and stepping upward algorithms. 


a. Stepping Forward Algorithm 


The leg, without bending at the knee, can be assumed as a simple swinging 


bar on a circular path which is shown as P , in Figure 16. The angle between the vertical 
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and the swinging bar is defined as OQ, which takes values between -20 and +20 degrees 
during the gait cycle. In reality, the knees are bent during stepping. Larger values of © 
introduce larger bending angles at the knee. When is equal to zero, knee bending angle 
should also be zero. This requirement is introduced by 
P. = P, cos (eq. 4.20) 
There is a difference between the supporting and the recovery leg motions. While 
the supporting leg always touches the ground, the recovery leg should be moved without 


touching the ground. This requirement is added to the model by using the following 


equation for the recovery leg 


P, = Wee (eq. 4.21) 





Figure 16: Forward Stepping Algorithm 


No kinematic computation is needed for the arms. They are synchronized 
with the legs. The arm rotation is not exactly the same as leg rotation; a scale factor is 
applied for a more realistic looking arm motion. Another property of the model is the 
rotation of the upper body around the vector described by the general direction of motion. 


This motion is caused by the roll moments which are generated by the nature of biped 
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locomotion since the supporting legs have offset from the center of mass on opposite sides 


and are alternating during each step cycle. 


| I . 
CI CI 
Figure 17: Upper Body Rotation 


The distance from the upper body to the ground is not constant. When the 


Q& parameter has larger values, the whole body gets closer to the ground. This property is 


implemented by moving the whole body vertically with a cosine function of time. 





Figure 18: Change in the Height of the Body During One Gait Cycle 


fe 


The height of the upper body is calculated by 


Height = 0.08 cos(27ft) (eq. 4.22) 


b. Stepping Upward Algorithin 
The stepping up algorithm has a major difference from the forward stepping 


which is caused by the need for increase in body elevation 


h 





+l, 


Figure 19: The Amount of Elevation For Stepping Up 


The needed elevation for stepping up Is 
h = (1, +1,)-(, +1,)cosB. (eq. 4.23) 


This elevation can only be handled by raising the foot to a higher position 1n the front. This 


causes the change which ts shown in Figure 20. The values of @ change from -10 to +30 


degrees for the gait cycle. 





Figure 20: Stepping Up Algorithm 


C. DYNAMIC COMPUTER MODELS 


1. Newton-Euler Rigid Body Class 


The Dynamic models in this thesis are developed in ANSI Common Lisp. The main 
class for the dynamic simulation is rigid-body class which was written by Professor 
McGhee. This Lisp code for this class is included as Appendix A of this thesis. This class 
defines a free rigid body in space. The major method of the rigid-body class 1s update-rigid- 
body which updates the posture vector which includes the position and the orientation of 
the rigid body in an earth coordinate system. This method converts body velocity rates to 
earth velocity rates to update the six element posture vector. Euler integration is used to 
update body velocities by using body velocity growth rates. The body velocity growth rates 
result from applied forces and torques on the body. The linear velocity growth rate is 


computed by [FRAN69] 


rv-qw+—-—gsind 
m 
Se des ° ? 
vl = | pw-ru+— + cosOsing (eq. 4.24) 
| m 
fy 
qu-py+—~ + gcosUcosd 


where f e f ? y = is the force vector applied to the body, (u, Vv, W) is the linear velocity 


vector, (p,q,r) is the rotational velocity vector, (@,, W) is the body orientation 


defined in Euler angles, mm is the mass, and g is the gravitational acceleration. All these 
vectors are defined in a body principal axis coordinate system [FRAN69]. The angular 


velocity growth rate 1s computed by 


[l= Tar + /6) 


ie 
i | ee +M 
g| = Lee Fex)P (eq. 4.25) 
I 
r vy 
Teed tN} 
is 


where (L, M, N) is the torque vector applied to the body, J, / oa and | 77 are mass 


moments of inertia, (Pp, g, r) is the rotational velocity vector [FRAN69]. In summary, the 
update-rigid-body method calculates the new position and orientation of the free rigid body 
in an earth coordinate system according to the applied forces and torques to the body. An 
other important method is move which takes the displacement components and orientation 


Euler angles as input and moves the body according to the inputs. 
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2. Numerical Integration Methods 


Two different integration methods are used in the dynamic modeling 
implementations: Euler and Huen integration. The Euler integration approach can be 


formalized as [KREY88] 
C4) =%,+ reese t At (eq. 4.26) 
where 


y= i Cm t) (eq. 4.27) 


and At is aconstant increment in the independent variable, t. 


The Heun integration formula can be defined with the equation [KREY88] 


] 
es aes 5 LE (Xp Da gi ae mare ie ee IA (eq. 4.28) 
where 


ee is =X, tf (ee t At. (eq. 4.29) 


The symbols Af and f (x, t _) have the same definitions for Heun integration as in Euler 


integration. 


3. Dynamic Inverted Pendulum Simulations 


Three different dynamic inverted pendulum models are implemented in this thesis: 


a. A Single Link Single Rigid Body with Newton-Euler 
The second implementation simulates an inverted pendulum by using Newton- 
Euler formulation of the dynamic equations of the system. The constraint forces from the 
dynamic equations and the control torque are the inputs of the rigid-body class. The 3x3 


matrix multiplication form of the system equations is as follows: 
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I —lcos®@ /sin®@ ia 


ey 
mlicos® | 0 P| = 8 misin® (eq. 4.30) 
_ 9 
vee! iF img —9 micos@! 


where all the variables are defined as in Figure 21. 





Figure 21: Inverted Pendulum with Constraint Forces 


The system has only one degree of freedom. A suitable state vector is 
x = [6,0] (eq. 4.31) 


To assume M = Q, makes the system behave like a natural inverted pendulum 


without control. By using linear state feedback, the control moment can be determined as 


M = -K,o- Ko (eq. 4.32) 


where K 6 and K A are control gain variables. Suitable values for K 6 and K é can be 


found analytically for small motion linearization of this system because its characteristic 


equation is quadratic. The Lagrangian formulation, Eq 2. 27,is most useful for this purpose. 
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b. Massless Leg and a Single Rigid Body with Lagrangian 

The second implementation simulates a two link structure which is 
presented in the third chapter of this thesis. The dynamic equations of the system are 
defined with Eq. 3.18 and Eq. 3.24 which are derived by using Lagrangian method. The 
linear state feedback control equations are given by the equations Eq. 3.6 and Eq. 3.7. The 


state vector is defined as Eq. 3.1. 


c. Massless Leg and a Single Rigid Body with Newton-Euler 

The third implementation simulates the same two link structure which is 
simulated in the second simulation. However the Newton-Euler method is used to derive 
the dynamic equation instead of the Lagrangian. The 3x3 matrix form of the dynamic 
equations are given by Eq. 3.42. The linear state feedback control equations and the state 


vector are defined as the same as in the Lagrangian version of the simulation. 


D. SUMMARY 

This chapter presents forward and upward kinematic stepping algorithms, of an 
human model which is developed at the IRIS Performer!™ application environment by 
using C++. The second part of the chapter explains three different implementations to 
simulate various inverted pendulum models developed in Lisp. The next chapter discuses 


the results of these simulations. 
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V. RESULTS OF COMPUTER SIMULATIONS 


A. INTRODUCTION 
In this chapter, the results of the dynamic simulations are presented. The second 
part of the chapter contains frames from the kinematic simulation of Dynaman which show 


the model stepping forward and upward. 
B. DYNAMIC SIMULATIONS 


1. A Single Link Single Rigid Body with Newton-Euler Method 
Figure 22, Figure 23, and Figure 24 show the behavior of the inverted pendulum 


with a control torque at the pivot point. The dynamic equations of the system are derived 
by using the Newton-Euler Method. The mass of the rigid body,m, is 100 lb. The body 
rotary inertia, /, is 900 lb. ft.2. The length of the inverted pendulum, /, is 3 ft. The 


gravitational acceleration, 2, 1S 32.2185 ft. / sec.”. The gain values for the control torque 


are 


10000 (eq. 5.1) 


Kg 


K. 


6 2000 (eq. 5.2) 


The initial state vector 1s 

x = (1,0) (ei) 
and the state vector in steady state is 

x = (0,0) (eq. 5.4) 

Figure 25 shows inverted pendulum orientation change in time. Euler integration 


with a time step of 0.02 seconds was used for the results shown in Figure 22-25. 
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Figure 22: Initial Position and Orientation of the Inverted Pendulum 
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Figure 23: Inverted Pendulum Moving to the Upright Orientation 
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Figure 24: Inverted Pendulum After Steady State 
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Figure 25: Body Attitude Response of the Inverted Pendulum 
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2. Massless Leg and a Single Rigid Body with Lagrangian Method 


Figure 26, Figure 27, Figure 28, and Figure 29 show the behavior of the two link 
inverted pendulum with control torques at the hip and at the ankle. The z axis of the body 
is drawn to be able to observe the body attitude and the leg angle separately. The dynamic 


equations of the system 1s derived by using the Lagrangian Method. The mass of the rigid 
body,m, is 100 Ib. The body rotary inertia, J, is 100 Ib. ft.7. The length of the leg, r, is 3 
ft. The length of the rigid body, /, is 0.5 ft. The gravitational acceleration, g, is 32.2 ft. / 


sec.”. The gain values for the control torques are 


Kg = 25000 (eq. 5.5) 

K = 12500 (eq. 5.6) 
] 

K 2, = 40000 (eq. 5.7) 

K, = 408 (eq. 5.8) 


The initial state vector 1s 
xX = (0: sate le 0) (eq. 5.9) 
and the state vector in Steady state 1s 
x = (On0R0 0) (eq. 5.10) 
Figure 30 and Figure 31 show body attitude and leg angle changes in time. Heun 


integration with a time step of 0.01 seconds was used for the results shown in Figure 26-31. 
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Figure 27: Two Link Inverted Pendulum Moving to the Upright Orientation by Control 
Torques(Lagrangian) 
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Figure 29: Two Link Inverted Pendulum After Steady State (Lagrangian) 
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Figure 30: Body Attitude Response of the Two Link Inverted Pendulum (Lagrangian) 
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Figure 31: Leg Angle Response of the Two Link Inverted Pendulum (Lagrangian) 
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3. Massless Leg and a Single Rigid Body with Newton-Euler Method 


Figure 32, Figure 33, Figure 34, and Figure 35 show the behavior of the two link 
inverted pendulum with control torques at the hip and at the ankle. Again, the z axis of the 
body is drawn to be able to observe the body attitude and the leg angle separately. The 


dynamic equations of the system are derived using the Newton-Euler Method. The mass of 
the rigid body,77, is 100 lb. The body rotary inertia, /, is 100 lb. ft.2. The length of the leg, 
r, is 3 ft. The length of rigid body, /, is 0.5 ft. The gravitational acceleration, g, is 32.2 ft. 


/ sec.*. The gain values for the control torques are 


Kg = 25000 (eq. 5.11) 
KG = 2500 (eq. 5.12) 
Ky, = 40000 (eq. 5.13) 
= 400 (eq. 5.14) 


The initial state vector 1s 


x = (0.5807 1.0) (eq. 5.15) 

and the state vector in Steady state is 
x = (OR0R0. 0) (eq. 5.16) 
Figures 32 through 37 show body attitude and leg angle changes in time. Heun 


integration with a time step of 0.01 seconds was used for these results. 


66 


Penh : : ; 
Two Link pect Pendulum (C (Consent L os Masstess I Leg) NEWTON EULER 





Figure 32: Initial Orientation of the Two Link Inverted Pendulum (Newton-Euler) 
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Figure 33: Two Link Pendulum Moving to the Upright Orientation (Newton-Euler) 
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Figure 34: Two Link Pendulum Recovering the Negative Orientations (Newton-Euler) 
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Figure 35: Two Link Inverted Pendulum In Steady State (Newton-Euler) 
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Figure 36: Body Attitude Time Response of the Two Link Inverted Pendulum (Newton- 
Euler) 
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Figure 37: Leg Angle Response of the Two Link Inverted Pendulum (Newton-Euler) 
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C. RESULTS OF THE DYNAMIC SIMULATION 

Euler integration was used for the single link inverted pendulum simulation. For the 
two link inverted pendulum simulations, Heun integration 1s chosen. Heun is a more 
accurate method than Euler, because it converges quadratically as the step size is decreased, 
while Euler integration converges only linearly. 

As seen in the previous section, the Newton-Euler and Lagrangian solutions of the 
two link inverted pendulum problem give identical results. However, the running times of 
these simulations are not the same. The time needed to complete the Newton-Euler version 
of the simulation is 20 percent longer than the Lagrangian version. This is an expected 
consequence of the matrix inversion which takes place in the Newton-Euler version. The 
Lagrangian version of the problem computes the accelerations without matrix inversion. 
However, it is hard to derive dynamic equations using Lagrangian methods for more 
complex models with higher degrees of freedom. Moreover, due to the complexity of such 
equations, it is suspected that Newton-Euler models for postural control may run faster for 


more complex systems. This will certainly be true if O(n) methods are used [MCMI95}. 
D. KINEMATIC SIMULATION OF STEPPING DYNAMAN 


1. Stepping Forward Algorithm 
Figures 38 through 43 are six frames from the kinematic model simulation. These 


six frames show one step cycle of Dynaman during forward stepping. 
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Figure 39: Forward Stepping (2) 
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Figure 41: Forward Stepping (4) 





Figure 42: Forward Stepping (5) 





Figure 43: Forward Stepping (6) 
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2. Stepping Upward Algorithm 


Figure 44 through 48 are five frames from the kinematic model simulation. These 


five frames show one step cycle of the Dynaman while stepping upward. 





Figure 44: Upward Stepping (1) 
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Figure 45: Upward Stepping (2) 




















Figure 46: Upward Stepping (3) 





Figure 47: Upward Stepping (4) 





Figure 48: Upward Stepping (5) 
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VI. SUMMARY AND CONCLUSIONS 


A. SUMMARY 

The increasing demand for realistic 3D Virtual Environments requires more 
research on modeling human motion. There exist two main approaches to this problem: 
kinematic and dynamic modeling. A detailed review of previous work on kinematic 
modeling, dynamic modeling and control of graphical representation of human body 
motion is presented in this thesis. Kinematics uses only the geometric constraints of the 
model which results in less complicated models. However, for some situations, kinematic 
modeling may not be sufficient to simulate human motion realistically. Dynamics 
introduces additional properties of human limb segments to the simulation, such as mass 
and moment of inertia. These additional properties allow a more realistic simulation. The 
cost of this realism is an increase in computational complexity. The goal of the research of 
this thesis is to develop a realistic looking human model, while considering the complexity 
of the simulation according to the response time based on the computational power of 
current graphics hardware. 

To be able to create a more realistic, real time, and computationally efficient human 
model, a simple dynamic model which was proposed, but not fully developed in 
[MCGH79] was implemented. The model is a two link planar inverted pendulum which is 
made of a rigid body with mass and a supporting massless leg. Two different methods, 
generalized coordinates with Lagrangian, and the Newton-Euler, were used to derive the 
dynamic equations of the simulations. The implementation results verified models against 
each other and related models in the literature [GUBI74]. The problem of determining the 
gain parameters for limb segment control torques is also investigated. Since the degree of 
the characteristic equation is four, an experimental solution is presented, instead of an 


analytic one. Another result is the difference between the running times of the simulations. 
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The simulation which uses the Newton-Euler method takes 20% longer than the 
Lagrangian version, because of matrix inversions. It is also observed that the Lagrangian 
differential equations are simpler to use in deriving gain values for the system in 
comparison to the Newton-Euler version of the problem. However, the Lagrangian method 
would be hard to use in a more complex model with higher degrees of freedom. For such 
cases, the recursive Articulated Body algorithm [MCMI95] which has O(n) complexity 
needs to be used. 

On the other hand, a detailed kinematic human model which contains 14 body parts 


was also developed. The simulation is based on joints angle instead of the Euler angles 
which used in previous work. IRIS Performer!™ API, which interfaces to both IRIS GL™ 


and OpenGL. IRIS Performer!™ and provides a hierarchy among Dynamic Coordinate 


Systems (DCS), was used to implement articulated chain structures easily. 


B. CONCLUSION AND FUTURE RESEARCH 

This thesis covers the basics for simplified dynamic human motion simulation. The 
model which is introduced in this thesis has only the torso with mass. An advanced version 
of this simulation may consider leg mass as well. The existing model is planar. Another step 
can be to investigate a 3D version of the model. The “Rigid-body” class could continue to 
be used for this purpose since provides support for simulation of body parts in 3D. 
Multilink more complex dynamic models also need to be investigated by considering 
different solution methods such as Newton-Euler, iterative use of inverse dynamics 
[KOOZ83], and the Articulated Body Algorithm [MCMI95]. Accuracy and time response 
of the system should be taken into account, while the complexity of the model is increased. 

Another issue is to integrate the existing kinematic and dynamic models. This 
integration may result a more realistic human model than the developed kinematic model. 


It will be computationally cheaper than a 14 link human dynamic model. 
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This thesis investigated posture control of a human model. Stepping is another 
important subject in human motion simulation. An efficient automatic stepping algorithm 
needs to be to investigated for a dynamically simulated human model. Active (dynamic) 
balancing should be considered while studying automatic stepping control. Advance 
features in further research in stepping is speed and direction control, and rough terrain 
locomotion for the walking human model. For direction and speed control 
implementations, user-computer interaction will become important. A speech recognition 
interface could be taken in the consideration as a realistic solution [DEVI96]. 

VRML is a fast growing 3D modeling language. It is possible to execute the needed 
computation by using Java Classes which are integrated to VRML nodes. Implementing the 
simulations by using VRML and Java Scripting would provide platform independency 
which would allow the simulations runable in other than graphic workstation 
environments. The author hopes that the work of this thesis will provide a foundation for 


continuing research in some of the above topics. 
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APPENDIX A: DYNAMIC SIMULATION SOFTWARE 


COMMON FILES IN ALL DYNAMIC SIMULATIONS 


KEK KKK KKK KKK KKK KEKE KK KKK KKK KEK KKK KKK KK KEKEKKE KEK KKK KK KK KK KK KKK KKK KKK KK KKK KK 


77 File >; robot-kinematics.cl 

PAU CHOY : Dr. R. McGhee 

iG : Naval Postgraduate School 

oo : Monterey, CA 93943 

i cummany : This file contains various matrix and vector operation 


3 ~SLUuNct ions. 
KKEKK KKK KKK KE KKK KKK KKE KK EKER KE KKK KEKEKKEKR KEKE KKK KKK KKK KKK KKK KKK KK KKK KKK KK KK KK 


(defun transpose (matrix) @A Matrix is a list of row vectors. 
Ccerde (au) (cdr imatrix)) (mapcar “list (car matrix) )) 
(t (mapcar ‘cons (car matrix) (transpose (cdr matrix)))))) 


(defun dot-product (vector-1 vector-2) j;A vector is a list of numerical atoms. 
(apply '+ (mapcar '* vector-1 vector-2))) 


(defun vector-magnitude (vector) (sqrt (dot-product vector vector) )) 


(defun post-multiply (matrix vector) 
(eond  ( (null (rest matrix)) (list (dot-product (first matrix) vector))) 
(es (cons 4aet—product. (first matrix) vector) 
(post-multiply (rest matrix) vector))))) 


(defun pre-multiply (vector matrix) 
(post-multiply (transpose matrix) vector) ) 


(defun matrix-multiply (A B) ;A and B are conformable matrices. 
(Sonmemcinull (cdr A)) (list (pre-multiply (car A) B))) 
(t (cons (pre-multiply (car A) B) (matrix-multiply (cdr A) B))))) 


(defun chain-multiply (L) ;L is a list of names of conformable matrices. 
(cond ((null (cddr L)) (matrix-multiply (eval (car L)) (eval (cadr L)))) 
(t (matrix-multiply (eval (car L)) (chain-multiply (cdr L)))))) 


(defun cycle-left (matrix) (mapcar ‘row-cycle-left matrix) ) 
(defun row-cycle-left (row) (append (cdr row) (list (car row)))) 


(defun cycle-up (matrix) (append (cdr matrix) (list (car matrix)))) 


(defun unit-vector (one-column length) ;Column count starts at 1. 
(dewlin. Lengthy (l= n).) 
(veecton mil (Cons (cond ((= one-column n) 1) (t 0)) vector))) 


fezebep m) vector) )) 


(defun unit-matrix (size) 
(do ((row-number size (l- row-number) ) 
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(I nil (cons (unit-vector row~-number size) I))) 
((zerop row-number) I))) 


(defun concat-matrix (A B) ;A and B are matrices with equal number of rows. 
(cond ((nulieaeee 
(t (cons (append (car A) (car B)) (concat-matrix (edr A) 
(cdr B)))0g8 


(defun augment (matrix) 
(concat-matrix matrix (unit-matrix (length fatri))) 


(defun normalize-row (row) (scalar-multiply (/ 1.0 (car row)) row) ) 


(defun scalar-multiply (scalar vector) 
(cond ( (ull vector joni!) 
(t (cons (* scalar (car vector) ) 
(scalar-multiply scalar (cdr vector)))))) 


(defun solve-first-column (matrix) ;Reduces first column to (1 0 [ee 
(do* ((remaining-row-list matrix (rest remaining-row-list) ) 
(first-row (normalize-row (first matrix) )) 
(answer (list first-row) 
(cons (vector-add (first remaining-row-list) 
(scalar-multiply (- (caar remaining-row-list) ) 
first-row) ) 
answer) ) ) 
((null (rest remaining-row-list)) (reverse answer) ))) 


(defun vector-add (vector-l vector-2) (mapcar ‘+ vector-1l vector-2) ) 


(defun vector-subtract (vector-l vector-2) (mapcar ‘- vector-1 vector-2) ) 
(defun first-square (matrix) ;Returns leftmost square matrix from argument. 
(do ((size (length matrix) ) 
(remainder matrix (rest remainder) ) 
(answer nil (cons (firstn size (first remainder)) answer) ) ) 
((null remainder) (reverse answer) ))) 


(define firstn “(melase. 
(cond ((zerop n) nmr) 
(t (cons (first list) “(firsten )— niet rest etic...) 


(defun max-car-firstn (n list) 
(append (max-car-first (firstn n list)) (nthcedr n list))) 


(defun matrix-inverse (M) 
(do ((M1l (max-car-first (augment M)) 


(Comda (nally Mi) ad) ;Abort for singular matric, 
(t (max-car-firstn n (cycle-left (cycle-up Ml)))))) 
(n (l1- (length M)) (l- n))) 
((or (minusp n) (null Ml)) (cond ((null Ml) nail) (t€ (first -squarec tye 
(setq M1 (cond ((zerop (caar M1)) nil) (t (solve-first-column M1)))))) 
(defun max-car-first (L) ;L is a list of lists. This function finds list) wae 
(cond-((null (cdr L)) L) ;largest car and moves it to head of list of lists. 
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(t (1f (> (abs (caar L)) (abs (caar (max-car-first (cdr L))))) L 
(append (max-car-first (cdr L)) (list (car L))))))) 


(defun dh-matrix (cosrotate sinrotate costwist sintwist length translate) 
(list (list cosrotate (- (* costwist sinrotate) ) 
(* sintwist sinrotate) (* length cosrotate) ) 
(list sinrotate (* costwist cosrotate) 
(- (* Sintwist cosrotate)) (* length sinrotate)) 
(list 0. Sintwist costwist translate) (list 0. 0. 0. 1.))) 


(defun homogeneous-transform (azimuth elevation roll x y 2z) 
Mette ((spost (Sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation) } 
(eeheteos  elevation)) (sphi (Sin roll)) (ephi (cos roll))) 
Giver tlictm scpsi ct) (- (* cpsi sth sphi) (* spsi cphi)) 
(ion eps. Sth Cphi) (* spsi. sphi)) x) 
Gece s0; Sspst ceh) (+ (* cpsi cphi) (* spsi sth “sphi) ) 
(sececiesth coi) (* cpsi. sphi)) y) 
West (— Sth) s¢*sctm sphi) @.cth cphi) 2) 
(listmemeOen02. 1. )))) 


(defun inverse-H (H) ;H 1s a 4x4 homogeneous transformation matrix. 
(Tee Cimanus-—-P (list (- (fourth (first H))) 

(- (fourth (second H))) 
few OuUreca (thira H))))) 

(inverse-R (transpose (first-square (reverse (rest (reverse H)))))) 

(inverse-P (post-multiply inverse-R minus-P) ) ) 
(append (concat-matrix inverse-R (transpose (list inverse-P))) 
Gmictemtist O7.0..0 1))))) 


(defun rotation-matrix (azimuth elevation roll) 
(let ((Sspsi (Sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation) ) 
(Grn coseclevation)) (Ssphi (sin roll)) (cphi (cos roll))) 
Ghrcteercite( ~ecosi1 Cth) (—- (* cpsi sth sphi) (* spsi cphi)) 
Cam cost Sth Cohni)s(* Sspsi sphi))) 
Glue spsi cth) (+> (* cpsi cphi) (* spsi sth sphi)) 
ae  Spsi. sth. ephi) (* epsi sphi))) 
Circe Ssehjmmeermeth Sohi js (* cth cphi))))) 


(defun body-rate-to-euler-rate-matrix (azimuth elevation roll) 
(let ((sth (sin elevation)) (cth (cos elevation)) (tth (tan elevation) ) 
(semaecam roll)) (cphi (cos roll))) 
Cetsterctm le (~ tth Sphi) (* tth cphi)) 
GCiict. Omepni (— sphi)) 
Geist -scpurecth) 4(/ cpohi cth))))) 


83 


e 
f 
i 


i 


° 
é 
. 
i 


, 


KKK KK KKK KKK KKK KKK KKK KK KKK KKK KKK KK KKK KKK KKK KKK KEK KEKE K KEK KKK KKK KKK KKK KKK 


File ~ Narmonic—-equation.cl 
AUENOL : Dr. R. McGhee 
Naval Postgraduate School 
Monterey, CA 93943 
Summary : This file contains functions to create a window and execute 


drawing operations on the window. 
KEK KK KKK KKK KK KEKE KKK KEKE KKK KKK KKK KK KEK KEKE KEKE KEKE KKK KKK KKK KKK KKK KKK KKK KKK KKK KK 


(require :xcw) 
(cw:initialize-common-windows) 


(defun make-window () 


(cw:make-window-stream :borders 5 
:leftee16 
-bortomez so 
:width 600 
:sheight 600 
stitile “Harmonie Boquatiwen. 
:activate-p t)) ;Make window visible. 


(defun scale-point-coordinates (x-y-list enlargement-factor) 


(let ((x (first x-y-list)) (y (second x-y-list))) 
(list (+ 50 (round (* enlargement-factor x))) 
(+ 225 (round (* enlargement-factor y)))))) 


(defun draw-coordinate-axes (window) 


(cw:draw-line window (cw:make-position :x 20 :y 225) 
(cw:make-position :x 570 :y 225) 
:brush-width 2) 

(cw:draw-line window (cw:make-position :x 50 :y 20) 
(cw:make-position :x 50 :y 560) 


:brush-width 2) ) 


(defun draw-line-in-window (window enlargement-factor start end) 


(let ((scaled-start (scale-point-coordinates start enlargement-factor) ) 
(scaled-end (scale-point-coordinates end enlargement-factor) )) 
(cw:draw-line window 
(cw:make-position :x (first scaled-start) :y (second scaled-start) ) 
(cw:make-position :x (first scaled-end) :y (second scaled-end))))) 
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SINGLE LINK INVERTED PENDULUM SIMULATION WITH NEWTON-EULER 


KKK KKK KKK KEK KKK KKK KK KKK KKK KKK KKK KKK KKK KKK KKK KR EK KKK KKK KR KKK KKK KKK 


;; File : euler-angle-rigid-body.cl 

7) BUCHOr : Dr. R. McGhee 

ot : Naval Postgraduate School 

a : Monterey, CA 93943 

;; Modified by : Mehmet Bediz 

;; Summary : This file contains rigid body class which is implemented 

7 : by Dr. R. McGhee. The function “test-rigid-body-forces-and- 


es : torques-three-with-M” and the other functions called by this 
ii : function are implemented by Mehmet Bediz. 


KKK KKK KKK KKK KKK KE KKK KKK KEK KKK KKK KKK KKK KKK KKK KK KKK KKK KKK KKK KKK KKK KKK 


(defclass rigid-body 
” 
( (posture ;The vector (xe ye ze phi theta psi). 
mnuenomn, (0 0 0-0 0 0) 
-initarg :posture 
:accessor posture) 
(posture-rate ;The vector (xe-dot ye-dot ze-dot phi-dot theta-dot psi-dot). 
:initarg :posture-rate 
:accessor posture-rate) 
(velocity ;The six-vector (u v wp qe) in body coordinates. 
piniterorm (070.0 60 0.0) 
sanitarg :velocity 
saccessor velocity) 
(velocity-growth-rate ;The vector (u-dot v-dot w-dot p-dot q-dot r-dot). 
:accessor velocity-growth-rate) 
(forces-and-torques ;The vector (Fx Fy Fz L MN) in body coordinates. 
mointerocnm list 0 O(—- *gqravity*) 0 0 0) 
Finite rornie(21st 0-0 0 0 0 0) 
:accessor forces-and-torques) 
(moments-of-inertia ;The vector (Ix Iy Iz) in principal axis coordinates. 
sent rorme (|) 900.1) 
:initarg :moments-of-inertia 
:accessor moments-of-inertia) 
(mass 
-inatrorm 100 
:initarg :mass 
:accessor mass) 
(node-list noe ze) ean bedy coord for each node. Starts with (0 0 0 1). 
Pienemann (Om Uo) (-O.5 -0.5 0.5 1) (0.5 -0.5 0.5 1) (0.5 -0.5 -0.5 1) 
(Oe ea oe Osa ly (-0-59055 0.5 1) (0.5 0.5 -0.5 1) 
(Om5ento. 5. =O 25 -1)-0.5. 025-055 1)(0 0 0 1) 
(COCs 1) ) 
:imitarg :node-list 
:accessor node-list) 
(polygon-list 
imine leo e 4) (5 6 7 8)(6 2 3 7)(2 1 4 3) (9 10)) 
:initarg :polygon-list 
saccessor polygon-list) 
(transformed-node-list ;(x y z 1) in earth coord for each node in node-list. 
ier OmOmOmilen(—-2 —-2 0 1) (2 -2 0.1) (2 =24-30 1) (-2 -2°-30 1) 
omen yt 2 ee) (2 eee oUt) (-20CU 2 = 30 1) 
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:accessor transformed-node-list) 
(H-matrix 

sinitform (untt-matrr.) 
:accessor H-matrix) 

(time-stamp 

:accessor time-stamp) ) ) 


(defmethod initialize ((body rigid-body) ) 
(setf (transformed-node-list body) (node-list body) ) 
(setf (velocity-growth-rate body) (update-velocity-growth-rate body) ) 
(setf (posture-rate body) (earth-velocity body) ) 
(setf (time-stamp body) (get-internal-real-time) ))) 


(defmethod set-transformed-node-list-z((body rigid-body) 2) 


(setf (third(fourth (transformed-node-list body)))(- z)) 
(setf (third(fifth (transformed-node-list body))) (- z)) 
(setf (third(eighth (transformed-node-list body))) (- z)) 
(setf (third(ninth (transformed-node-list body))) (- z)) 


(transformed-node-list body) )) 


(defun transform (obj) 
(last 0 (fist obj) (Second obj) (thard obj) 


(defun reverse-transform (obj) 
(list (second obj) (third obs )i{fourth obs) 


(defmethod move ((body rigid-body) azimuth elevation roll x y 2) 
(setf (posture body) (list x y z roll elevation azimuth) ) 
(setf (H-matrix body) 

(homogeneous-transform azimuth elevation roll x y 2z)) 
(transform-node-list body) ) 


(defmethod get-delta-t ((body rigid-body)) 0.02) 


: (let* ((new-time (get-internal-real-time) ) 

: (delta-t (/ (- new-time (time-stamp body)) 1000))) 

; (setf (time-stamp body) new-time) 

: delta-t)) 

(defmethod update-rigid-body ( (body rigid-body) ) ;Euler integration. 


(let* ((delta-t (get-delta-t body) )) 

(update-posture body delta-t); EULER 

(update-velocity body delta-t); EULER 

(setf (H-matrix body) (homogeneous-transform (sixth (posture body) ) 
(fifth (posture body)) (fourth (posture body)) (first (posture beams 
(second (posture body)) (third (posture body)))) 

(transform-node-list body) 

(update-velocity-growth-rate body) ) ) 


(defmethod update-velocity-growth-rate ((body rigid-body) ) 
(setf (velocity-growth-rate body) ;Assumes principal axis coordinates with 


(multiple-value-bind ;origin at center of gravity of body. 
(Fx Fy FZ L MN U V Wp Ger Ix Iy Iz) ;Declares local variables. 
(values-list ;Values assigned. 

(append 


(forces-and-torques body) (velocity body) (moments-of-inertia body) )) 
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(list (+ (* vr) (* -1wq) (/ Fx (mass body) ) 
( Geewaey (first (third (H-matrix body) )))) 

CEN ew Dp) Seer) 6 (/ FY? (mass body) ) 
(* *gravity* (second (third (H-matrix body))))) 

eee eG) te ep) (/ Fz (mass body) ) 
Cora teveetehnird (third (H-matrix body))))) 


Wate = (—-3y 512) g +). L) Ix) 
Woe (> £2) Ix) © py) M) Ivy) 
(a ee er eV oma) N) 12))))) ;Value returned. 


(defmethod update-velocity ( (body rigid-body) delta-t) ;Euler integration. 
(set£f (velocity body) 
(vector-add (velocity body) 
(scalar-multiply delta-t (velocity-growth-rate body) )))) 


(defmethod update-posture ((body rigid-body) delta-t) ;Euler integration. 
{setf (posture-rate body) (earth-velocity body) ) 
(setf (posture body) 
(vector-add (posture body) (scalar-multiply delta-t (posture-rate body) )))) 


(defmethod transform-node-list ((body rigid-body) ) 
(setf (transformed-node-list body) 
(mapcar #'(lambda (node-location) 
(post-multiply (H-matrix body) node-location) ) 
(node-list body)))) 


Pactconstant *“gravity* 32.2185) 


(defmethod earth-velocity ((body rigid-body) ) 
(let* ((linear-velocity (firstn 3 (velocity body) )) 
(rotational-velocity (cdddr (velocity body) ) ) 
(posture (posture body) ) 
(R-matrix (rotation-matrix (sixth posture) (fifth posture) 
(fourth posture) ) ) 
(linear-earth-velocity (post-multiply R-matrix linear-velocity) ) 
(T-matrix (body-rate-to-euler-rate-matrix (sixth posture) 
(fifth posture) (fourth posture) )) 
(rotational-earth-velocity (post-multiply T-matrix 
rotational-velocity) )) 
(append linear-earth-velocity rotational-earth-velocity) )) 


(defun test-rigid-body () 
(setf airplane-1 (make-instance 'rigid-body) ) 
(initialize airplane-1) 
(setf camera-1l (make-instance ‘strobe-camera) ) 
(move camera-1 (- (/ pi 2)) 0 00 300 ) 
({take-picture camera-1l airplane-1) 
(dotimes (i 20 ‘done) (update-rigid-body airplane-1) ) 
{take-picture camera-1l airplane-1) ) 
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; - Inverted Pendulum (fixed leg length) 
; - state vector x = (theta theta-dot) 
; - state vector u = (M) 
; - control torque at the pivot (M) 
- Newton-Euler Method 
~ 3x3 matrix inversion to calculate forces and moments 
; - RIGID Body class to compute accelerations 
; - Heun Integration 
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(defmethod forces-earth-to-body (Fx-e Fy-e Fz-e theta) 
(let* ((forces-earth (list Fx-e Fy-e Fz-e)) 


(R-matrix (rotation-matrix 0 (- theta) 0)) ;Since planar 
(forces-body (post-multiply R-matrix forces-earth) ) ) 


forces-body) ) 


(defmethod update-forces-and-torques-three-with-M ((body rigid-body) M) 
(let* ((equations-solution-vector (solve-three-equation-system-with-M 
body M)) 
(Fx-earth (second equations-solution-vector) ) 
(Fz-earth (third equations-solution-vector) ) 
(F-body (forces-earth-to-body Fx-earth 0 Fz-earth 
(fifth (posture bodyjjiee 
(setf forces-and-torques (list (first F-body) 
0 
(third F-body) 
0 
(* (first equations-solution-vector) 
(second (moments-of-inertia body) ) ) 
0) =) 


(defun solve-three-equation-system-with-M (body M) 


(setf£f m (mass body) ) 

(setf 1 2) 

(setf I (second (moments-of-inertia body) )) 
(setf theta (fif=th (posture body) )) 

(setf theta-dot (fifth (velocity body) )} 

(setf g “Gravity 2) 


(post-multiply (matrix-inverse (list (list I (* -1 1 (cos theta) ) 
(* 1) (sin. theta) ))) 
(list (* m 1 (cos theta) ) i 01} 
(list (* m 1 (sin theta) 0 -1))) 
(list M 
(* (sqr theta-dot) m1 (sin theta) ) 
(- (* mg) (* (sqr theta-dot) m 1 (cos theta) ) jive 
(defun compute-M (body K-theta K-theta-dot) 
(setf theta (fifth (posture ~bedyv)) 
(setf theta-dot (fifth (velociey body) )} 
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(+ (* -1 K-theta theta) (* -1 K-theta-dot theta-dot))) 


(defun test-rigid-body-forces-and-torques-three-with-M (K-theta K-theta-dot) 
(setf inverted-pendulum (make-instance ‘rigid-body) ) 
(initialize inverted-pendulum) 
(setf camera-l (make-instance ‘strobe-camera) ) 
(move camera-1l (deg-to-rad (- 90)) (deg-to-rad 0) (deg-to-rad 0) 0 10 QO) 
(move inverted-pendulum 0 -1 0 0 0 Q) 
; (take-picture camera-1l inverted-pendulum) 


(setf graph-l-window (make-window-graph-1) ) 
(draw-coordinate-axes graph-1-window) 


(set£ old-theta 0.5) 
(oem amo: (+a) 1) ) ) 
Gie> 271000) 'end) 


(setf (forces-and-torques inverted-pendulum) 
(update-forces-and-torques-three-with-M inverted-pendulum 
(compute-M inverted-pendulum K-theta K-theta-dot) )) 
(update-rigid-body inverted-pendulum) 
(cw:clear (camera-window camera-1) ) 
(take-picture camera-l inverted-pendulum) 
(draw-line-in-window graph-l-window 80 


irs 
(* (get-delta-t inverted-pendulum) (- 1 1)) 
(* -1 old-theta) ) 

Glas e 
Co (get-delta-t inverted-pendulum) i) 


(* -1 (fifth (posture inverted-pendulum) )))) 
(setf old-theta (fifth (posture inverted-pendulum) )))) 


(defun sqr(x) 
(ex x) ) 


(defun make-window-graph-i () 
(cw:make-window-stream :borders 5 

sbeft 0 
—Doctom i950 
:width 450 
zsheight 450 
:title "Body Attitude (theta) NEWTON-EULER" 
-activate-p t)) ;Make window visible. 
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;; File : load-euler-files.cl 

7; Auther : Mehmet Bediz 

fas : Naval Postgraduate School 

= 5 : Monterey, CA 93943 

;; Summary : This file contains fEumctiens to load) titec and costes 
i : the simulations. 


KRREKKKKKKKEK KKK KKK KK KKK K KERR KEK KKK KEKE KKEK KEK KKK KEKEKKEKEKKKEKKRKKRKEKKK KKK KKK KEK 


-_ 2©« © © «© © © © © © © © ©» © © © © © © © © & © © © © © © & © © © © © & © © © = © © © © © © ee © ee ee ee © ee ee ee we ewe ewe ee © ee we eC we ee © 


ete t €@€ tt Ott tt ttt het rere’) 6 tt ee et tt tt 0 6 8 tt Oe Ok aA EY 8 AE EEE EAE EO AN SO Oe tate teeter tele 


; Load files 


ee + ® © © *& © © © © © © © © © © © © ee — &— &— © © © © e— © ee — © ee — © — © © — © © © © ee © &e © © ee ee we we we eC we ee we ee ee we ee 


ett € &@ tt € & & tt © 8 €or eee)’ te tte bee) bb bet et Ob eee TOE EG TO OTE EEE Ot CE tS LE Ee tate ii ete tee 


(load “harmonic-equation.cl") 
(load “robot-kinematics.cl") 

(load “euler-angle-rigid-body.cli") 
(load "strobe-camera.cl") 


> «© © © © © © © © © © © © © © © © © © ee © ee ee ee © ee ee ee ee ee © e— © F&F © © © © © © © © © © © © © © ee ee ee ee ee we we we ee ewe He 


tet 8 # # € 8 f tf 0 6 tt et tt ee ea a a a ETE EE TEE ee EE ft Ee ht ek te ei ett et fe eae eee 


: Tests 


> © «© * © © * ®* © © *® © © © © ®@ © & © & = = © © & © — — ee © © ee © — © — © © © — © Fe © ee Fe ee ee ee ee ee ee ee ee eee ee 


a a J A Ar, ee et dey ee ey ee ee ee a Le J a Ze ee ee ee ee ey Jee ee et Qe tee en Le dt et ow At eG een ete Maree Pee Te RP ET eee tet te EP eB Me CTP EM ae ERR Ee FT Ph Ee te Ey 


(defun graph_1 () 
(solve-robot-attitude 0.01 1.0 10000 2000 109660 20000) ) 


(defun graph_2 () 
(solve-robot-altitude-z_z0 0.01 1.0 10000 2000 109660 20000) ) 


(defun graph_3 () 
(solve-robot-altitude-z 0.01 1.0 10000 2000 109660 20000) ) 


(defun draw-converted-pendulum-test-real-parameters () 
(draw-converted-pendulum 0.01 2.0 10000 2000 109660 20000) ) 


(defun draw-converted-pendulum-test-zero-gain () 
(draw-converted-pendulum 0.01 2.0 0 0 0 0) ) 
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i; File : strobe-camera.cl 

4 pay y ble) evebg : Dr. R. McGhee 

NG : Naval Postgraduate School 

ay : Monterey, CA 93943 

;; Summary : This file contains strobe-camera class definitions. 


KKK KKK KKK KK KKK KK KKK KK KK mK RK KK KK KKK KKK KKK KK KK KKK KKK KKK KK KKKK K 


(require :xcw) 
(cw: initialize-common-windows) 


(defclass strobe-camera (rigid-body) 
((focal-length 
:accessor focal-length 
simnitrorm 6) 
(camera-window 
:accessor camera-window 
:initform (cw:make-window-stream :borders 5 
Lett 500 
“Dottom a0 ¢ 
rwadth 650 
sheight 650 
:title "“strobe-camera-image" 
sactivate-p t)) 
(H-matrix 
:initform (homogeneous-transform .3 -.3 0 -300 -90 -90)) 
(inverse-H-matrix 
:accessor inverse-H-matrix 
:initform (inverse-H (homogeneous-transform .3 -.3 0 -300 -90 -90))) 
(enlargement-factor 
z:accessor enlargement-factor 
ime form. 30) )) 


(defmethod move ((camera strobe-camera) azimuth elevation roll x y z) 
(setf (H-matrix camera) (homogeneous-transform azimuth elevation roll x y 2z)) 
(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera) ))) 


(defmethod take-picture ((camera strobe-camera) (body rigid-body) ) 
(let ((camera-space-node-list (mapcar #' (lambda (node-location) 
(post-multiply (inverse-H-matrix camera) node-location) ) 
(transformed-node-list body) ))) 
(dolist (polygon (polygon-list body) ) 
(clip-and-draw-polygon camera polygon camera-space-node-list) ))) 


(defmethod clip-and-draw-polygon 

((camera strobe-camera) polygon node-coord-1list) 

(do* ((initial-point (nth (first polygon) node-coord-list) ) 
(from-point initial-point to-point) 
(remaining-nodes (rest polygon) (rest remaining-nodes) ) 
(to-point (nth (first remaining-nodes) node-coord-1list) 

(if (mot (null (first remaining-nodes) )) 
(nth (first remaining-nodes) node-coord-list)))) 
((nmull to-point) 
(draw-clipped-projection camera from-point initial-point) ) 

(draw-clipped-projection camera from-point to-point))) 


9} 


(defmethod draw-clipped-projection ((camera strobe-camera) from-point to-point) 
(cond ((and (<= (first from-point) (focal-length camera) ) 
(<= (first to-point) (focal-length camera))) nil) 
((<= (first from-point) (focal-length camera) ) 
(draw-line-in-camera-window camera 
(perspective-transform camera (from-clip camera from-point to-point) ) 
(perspective-transform camera to-point) ) ) 
((<= (first to-point) (focal-length camera) ) 
(draw-line-in-camera-window camera 
(perspective-transform camera from-point) 
(perspective-transform camera (to-clip camera from-point to-point)))) 
(t (draw-line-in-camera-window camera 
(perspective-transform camera from-point) 
(perspective-transform camera to-point))))) 


(defmethod from-clip ((camera strobe-camera) from-point to-point) 
(let ((scale-factor (/ (- (focal-length camera) (first from-point) ) 
(= (£1PTSE2bo-point) (first Eroem—-poime as 
(list (+ (first from-point) 


(* scale-factor (- (first to-point) (first from-point)))) 
(+ (second from-point) 
(* scale-factor (- (second to-point) (second from-point)))) 
(+ (third from-point) 
(* scale-factor (- (third to-point) (third from-point))) jee 


(defmethod to-clip ((camera strobe-camera) from-point to-point) 
(from-clip camera to-point from-point) ) 


(defmethod draw-line-in-camera-window ((camera strobe-camera) start end) 
(cw:draw-line (camera-window camera) 
(cw:make-position :x (+ 150 (first start)) :y (+ 150 (second 
Stare) )) 
(cw:make-position :x (+ 150 (first end)) :y (+ 150 (second 
end) ) ) 
:brush-width 0)) 


(defmethod perspective-transform ((camera strobe-camera) point-in-camera-space) 


(let* ((enlargement-factor (enlargement-factor camera) ) 

(focal-length (focal-length camera) ) 

(x (first point-in-camera-space)) ;x axis is along optical axis 
(y (second point-in-camera-space)) ;y is out right side of camera 
(z (third point-in-camera-space))) ;z is out bottom of camera 
(list (+ (round (* enlargement-factor (/ (* focal-length y) x))) 

150) ;to right in camera window 
(+ 150 (round (* enlargement-factor (/ (* focal-length (- z)) x)) 


))))) ;up in camera window 


(defun test-camera (z theta) ;Produces top view of default rigid-body. 
(setf robot-leg (make-instance ‘rigid-body) ) 
(Initialize robot-leg) 
(set-transformed-node-list-z robot-leg 2) 
(set-transformed-node-list-theta robot-leg theta) 
(setf camera-1l (make-instance ‘strobe-camera) ) 
(move camera-1 (deg-to-rad 0) (deg-to-rad 0) (deg-to-rad 0) -30 0 Q) 


ae 


(take-picture camera-1l robot-leg)) 


(defun deg-to-rad (angle) (* .01745329251994329 angle) ) 
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TWO LINK INVERTED PENDULUM SIMULATION WITH LAGRANGIAN 
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7> File >: euler-angle-rigid-body.cl 

eee Veh alate pa : Dr. R. McGhee 

2s : Naval Postgraduate School 

aa : Monterey, CA 93943 

;; Modified by : Mehmet Bediz 

| 7 eoummar : This file contains rigid body class which is implemented 
a >: by Dr. R. McGhee. The function “draw-inverted-pendulum” 
; >: and the other functions called by this 

ce : function are implemented by Mehmet Bediz. 


KEKKKEKKKEKKKKEKKKKRKEKKEKKRKEKKKEKEKKEKKRKEKKKKEKEKEKK KKK KKK KKK KKK KK KKK KK KKK 


(defclass rigid-body 
() 
( (posture ;The vector (xe ye ze phi theta psi). 

Pint torm “200200070 sOmOn 

:initarg :posture 

:accessor posture) 
(posture-rate ;The vector (xe-dot ye-dot ze-dot phi-dot theta-dot psi-dot). 
:initarg :posture-rate 

z:accessor posture-rate) 
(VElOCT EY ;The six-vector (u vwpqr) in body coordinates. 
sinittorm. (0-0 SC aC moo) 

Sititearg :veloci ty. 

:accessor velocity) 
(velocity-growth-rate ;The vector (u-dot v-dot w-dot p-dot q-dot r-dot). 
:accessor velocity-growth-rate) 
(forces-and-torques ;The vector (Fx Fy Fz L MN) in body coordinates. 
sinittorm (last 0907050 0.0) 

:accessor forces-and-torques) 
(moments-of-inertia ;The vector (Ix Iy Iz) in principal axis coordinates. 
sinitrtorm =. (2 shoo) 

:initarg :moments-of-inertia 

:accessor moments-of-inertia) 
(mass 

‘inilttorun TCC 

:initarg :mass 

:accessor mass) 
(node-list ;(x y z 1) in body coord for each node. Starts with (0 ONGeiae 
sinitform *((0 0 0 1) (-0.5 -0.5 ©.5 1) (0.5 -0.5 0.5 1)(0.3)-023). 2 

(-0.5 -0.5 -0.5 1)(-0.5 0.5 0.5 1) (0.5 O75 303s 
(0.5 0.5 -0.5 1)(-0.5 0.5 -0.5 1)(0 0 O 1) (0) Ouekeae 

:initarg :node-list 

:accessor node-list) 

(polygon-list 

-initform *((1.5 8 4){5 6 7 8) (6.2 see es oe 

sinitarg :polyqon-liset 

:accessor polygon-list) 

(transformed-node-list ;(x y z 1) in earth coord for each node in node-list. 
sinitform °*((0 0:0 1) =(=]2 270 ye 0 i ee 

(-2 2 0 1) (25 250 2)(2. 2-302) (=2) 2-307 

:accessor transformed-node-list) 

(H-matrix 
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sini clorm eum t-matrix 4) 
:accessor H-matrix) 
(time-stamp 

:accessor time-stamp) ) ) 


(defmethod initialize ((body rigid-body) ) 
(setf (transformed-node-list body) (node-list body) ) 
(setf (velocity-growth-rate body) (update-velocity-growth-rate body) ) 
(setf (posture-rate body) (earth-velocity body) ) 
(setf (time-stamp body) (get-internal-real-time) ))) 


(defmethod set-transformed-node-list-z((body rigid-body) 2z) 


(setf (third(fourth (transformed-node-list body))) (- z)) 
(setf (third(fifth (transformed-node-list body))) (- z)) 
(setf (third(eighth (transformed-node-list body))) (- z)) 
(setf (third(ninth (transformed-node-list body))) (- z)) 


(trans formed-node-list body) ) ) 


(defun transform (obj) 
(Preset Oe(tirst obj) (Second obj) (third obj) )) 


(defun reverse-transform (obj) 
Meets seconda cd)) (third obj) (fourth obj) 1)) 


(defmethod move ((body rigid-body) azimuth elevation roll x y 2z) 
(setf (posture body) (list x y z roll elevation azimuth) ) 
(setf (H-matrix body) 

(homogeneous-transform azimuth elevation roll x y 2z)) 
(transform-node-list body) ) 


(defmethod get-delta-t ((body rigid-body)) 0.005) 


(defmethod forces-earth-to-body (Fx-e Fy-e Fz-e theta) 
(let* ((forces-earth (list Fx-e Fy-e Fz-e)) 


(R-matrix (rotation-matrix 0 (- theta) 0)) 
(forces-body (post-multiply R-matrix forces-earth) ) ) 


forces-body) ) 


(defmethod update-rigid-body ((body rigid-body) ) ;Euler integration. 
(let* ((delta-t (get-delta-t body) )) 

(update-posture body delta-t); EULER 

(update-velocity body delta-t); EULER 

(setf (H-matrix body) (homogeneous-transform (sixth (posture body) ) 
(fifth (posture body)) (fourth (posture body)) (first (posture body) ) 
(second (posture body)) (third (posture body)))) 

(transform-node-list body) 


(update-velocity-growth-rate body) )) 
(defmethod update-velocity-growth-rate ((body rigid-body) ) 
(setf (velocity-growth-rate body) ;Assumes principal axis coordinates with 


(multiple-value-bind ;origin at center of gravity of body. 
(Poa, by F2 L M N iewvewap or Ix Iy Iz) ;Declares local variables. 


25 


(values-list ;Values assigned. 
(append 
(forces-and-torques body) (velocity body) (moments-of-inertia body) ) ) 
(list (+ (* v vy) (* =1 wig) Fx (mass bea, 
(* *gravity* (first (third (A-matrix boa.) 
(+ (* wp) (* -l ur) (/ Fy (mass body)) 
(* *gravity* (second (third (H-matrix body))))) 
(+ (* u gq) (* -1l v p) (/ Fz (mass body) ) 
(* *gravity* (third (third (H-matrix ed, o0)) 


(f (HO = Dy eae ee) 
(/ (#9(* (= 1205 ae eo 
(/ (+ (* (- Ix Iy) pq) N) Iz))))) ;Value returned. 


(defmethod update-velocity ((body rigid-body) delta-t) ;Euler integration. 
(setf (velocity body) 
(vector-add (velocity body) 
(scalar-multiply delta-t (velocity-growth-rate body) )))) 


(defmethod update-posture ((body rigid-body) delta-t) ;Euler integration. 
(setf (posture-rate body) (earth-velocity body) ) 
(setf (posture body) 
(vector-add (posture body) (scalar-multiply delta-t (posture-rate body))))) 


(defmethod transform-node-list ((body rigid-body) ) 
(setf (transformed-node-list body) 
(mapcar #'(lambda (node-location) 
(post-multiply (H-matrix body) node-location) ) 
(node-list body) ))) 


(deétconstant “gravity 32.7265) 


(defmethod earth-velocity ((body rigid-body) ) 
(let* ((linear-velocity (firstn 3 (velocity body) )) 
(rotational-velocity (cdddr (velocity body) )) 
(posture (posture body) ) 
(R-matrix (rotation-matrix (sixth posture) (fifth posture) 
(fourth posture) ) ) 
(linear-earth-velocity (post-multiply R-matrix linear-velocity) ) 
(T-matrix (body-rate-to-euler-rate-matrix (sixth posture) 
(fifth posture) (fourth posture) )) 
(rotational-earth-velocity (post-multiply T-matrix 
rotational-velocity) )) 
(append linear-earth-velocity rotational-earth-velocity) )) 


(defun test-rigid-body () 
(setf airplane-l (make-instance '‘'rigid-body) ) 
(initialize airplane-1) 
(setf camera-1 (make-instance 'strobe-camera) ) 
(move camera-1 (- (/ pi 2)) 0 00 30 0 ) 
(take-picture camera-1 airplane-1) 
(dotimes (i 20 '‘done) (update-rigid-body airplane-1) ) 
(take-picture camera-1 airplane-1) ) 
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: - Two Link Inverted Pendulum 


oso ee © @ © © © © © © © © @ © © © © © © & @ 


; - Generalized Coordinates with Lagrangian Method 
; - Massless leg and a single rigid body (fixed leg length) 


; - State vector x = 
; - State vector u = (M-1 M-2) 

; - control torque at the ankle (M-1) 
; - control torque at the hip (M-2) 

; - Heun Integration 

; - By Mehmet Bediz 


Ce ee ee ee ae ee eS sc 6 ee ie ee) oe e+ es ee ¢ @« © = € © 6 © «© @ ©@ © 8» @ @ © 6 8&8 @ @ 6 © © ee we we wee hl hl hl hl hl hl hl hl Ul etl 


(set£ 1 O25} 
(setf rx 5) 
(setf time-step 0.01) 


(defun euler-step-constant-leg (state-vector M-1l M-2) 
(mapcar ‘+ state-vector (scalar-multiply time-step 
(derivative-state-vector 
state-vector M-1i M-2)))) 


(x K-theta-1 K-theta-1-dot 
K-theta-2 K-theta-2-dot) 


(defun heun-step-constant-leg 


(theta-1 theta-1-dot theta-2 theta-2-dot) 


(setf£ M-1 (compute-M-1 x K-theta-1 K-theta-1l-dot 
K-theta-2 K-theta-2-dot) ) 
(set£ M-2 (compute-M-2 x K-theta-1 K-theta-1-dot 


K-theta-2 K-theta-2-dot) ) 


(mapcar ‘+ x (scalar-multiply (* time-step .5) 
(vector-add (derivative-state-vector x M-1 M-2) 
(derivative-state-vector 


(euler-step-constant-leg x M-1 M-2) M-1 M-2))))) 


(defun derivative-state-vector (state-vector M-1 M-2) 

(list (second state-vector) 
(compute-theta-1-double-dot state-vector 
(fourth state-vector) 
(compute-theta-2-double-dot state-vector 


(defun compute-theta-1-double-dot (state-vector M-1 M-2) 


(set£ m 100) 
(setf 1 O25) 
(setf rx 8) 
(sett I 100) 
(setf theta-1 (first state-vector) ) 
(setf£ theta-1l-dot (second state-vector) ) 
(setf theta-2 (third state-vector) ) 
(setf theta-2-dot (fourth state-vector) ) 
(setf g 82.2) 
( / (+ (* 1 1lmmerg theta-2) 
(* -il mgr (+I (* mil i)) theta-1) 
ee ter (* m ft i)) M-1) 
cree © lemme) £ (* mi 1)) M-2) 


) 
(~S=i1°1 mr x) )) 


oF 


* 
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M-1 M-2) 
M-1 M-2))) 


ite eeone 


(defun compute-theta-2-double-dot (state-vector M-1 M-2) ; Eq 3.54 


(setf m 100) 

(setf 1 0253 

(setf r 3) 

(setf I 100) 

(setf£ theta-l (first state-vector) ) 
(setf theta-l-dot (second state-vector) ) 
(setf theta-2 (third state-vector) ) 
(setf theta-2-dot (fourth state-vector) ) 


(setf g B22) 


( / (+ (* mgil xr theta-2) 
(* -1 mgil sr theta-1) 
(* -1 1 M-1) 
(* (3 1 Ma2) 
) 
(<r) 


(defun compute-M-1 (state-vector K-theta-1 K-theta-l-dot 
K-theta-2 K-theta-2-dot) 
(setf theta-1 (first state-vector) ) 
(setf theta-l-dot (second state-vector) }) 
(+ (* -1 K-theta-1 theta-1) (* -1 K-theta-l-dot theta-1-dot) )) 


(defun compute-M-2 (state-vector K-theta-1l K-theta-l-dot 
K-theta-2 K-theta-2-dot) 
(setf theta-2 (third state-vector) ) 
(setf theta-2-dot (fourth state-vector) ) 
(+ (* -1 K-theta-2 theta-2)(* -1 K-theta-2-dot theta~-2-dot) )) 


(defun compute-y (state-vector) 


(setf theta-1l (first state-vector) ) 
(setf theta-1l-dot (second state-vector) ) 
(setf theta-2 (third state-vector) ) 
(setf theta-2-dot (fourth state-vector) ) 
(setf 1l O25) 

(setf r 3) 


(+ (* x (sin theta-1)) 
(* 1 (sin theta-2)))) 


(defun compute-z (state-vector) 


(setf theta-1 (first state-vector) ) 
(setf theta-l-dot (second state-vector) ) 
(setf£f theta-2 (third state-vector) ) 
(setf theta-2-dot (fourth state-vector) ) 
(setf 1 O25) 

(setf xr 2) 


(+ (* xr (cos theta-1) ) 
(* 1 (cos theta-2)))) 


(defun draw-inverted-pendulum (halt-time K-theta-1 K-theta-1-dot 
K-theta-2 K~theta-2-dot) 
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(setf robot-body (make-instance ‘rigid-body) ) 
(initialize robot-body) 


(setf robot-leg (make-instance ‘rigid-body) ) 
(initialize robot-leg) 


(setf camera-l (make-instance '‘strobe-camera) ) 
(move camera-l (deg-to-rad 0) (deg-to-rad 0) (deg-to~rad 0) -10 0 0) 


(setf graph-l-window (make-window-graph-1l) ) 
(draw-coordinate-axes graph-1-window) 


(setf graph-2-window (make-window-graph-2) ) 
(draw-coordinate-axes graph-2-window) 


(do ((x '(0.5 01.0 0) (heun-step-constant-leg x K-theta-l K-theta-l-dot 
K-theta-2 K-theta-2-dot) ) 
(ela-xe "(025 0 1.0 0) x} 
(time 0 (+ time 1))) 
((> time halt-time) ‘done) 


(move robot-body (deg-to-rad 0) (deg-to-rad 0) (third x) 
0 (compute-y x) (* -l(compute-z x))) 


(setf (transformed-node-list robot-leg) (list (list 0 0 0 1) 
(ast OM =r (sin (first: x) )) 
(S19 rr (eos sCiirst 2) )). 1) 
(ia Se 2000s). 

(setf£f (polygon-list robot-leg) (list (list 2 1))) 


(cw:clear (camera-window camera-1) ) 
(take-picture camera-1l robot-body) 
(take-picture camera-l robot-leg) 
(draw-line-in-window graph-l-window 80 (list (* time-step (- time 1)) 
(first old-x) ) 
(list (* time-step time) 
(ides cas) )) 
(draw-line-in-window graph-2-window 80 (list (* time-step (- time 1)) 
(third ofda-x)} 
(list (* time-step time) 
Coa ra. sy) 


(defun make-window-graph-1 () 

(cw:make-window-stream :borders 5 
:left 0 
HOGist OM = 
‘width 450 
sheight 450 
:title "Leg Angle (theta-1) LAGRANGIAN" 
:activate-p t)) -Make window visible. 


(defun make-window-graph-2 () 
(cw:make-window-stream :borders 5 
:sleft 0 


oe) 


*bottonesa0 


:width 450 

:height 450 

*title “Body Arla rtud— (theta-2) LAGRANGIAN" 
:activate-p t)) ;Make window visible. 
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File : load-euler-files.cl 
= Ss AuUthor Mehmet Bediz 
Naval Postgraduate School 


=e 
=e 


ee 


a : 
.3 : Monterey, CA 93943 

;; Summary ~telsoe tile contains £Lunctions to load files. 

° KKK KEKE KK KKK KK KK KK KKK KKK Ke KKK KKK KKK KEKE KKK KK KEKE KK KKK KKK KKK KEKE KEK KK KK KK 
oe? 


eee © @ © © © e@ @ . “ef 28 @ -_ cs © © © @ = © «© @ © © © © © © © © © © © © © © © © © © © 


ee eh etmhmUmUh OhUh oe 8 fr @ * ° 4 e 8 . 
cht) CP ER PE te PPE Pt TT ee Ht et Hr i et et et et et ey A St a ee oe At A et A On J a 2 i re A 2 2 A et Zr ee 2 


; Load files 


° ee «“ e©«© ee «© © © © © © © @& © © © © © © © © © © © © © © © © © © © © © & © © © © © © © © © © © © © © © © © © © © © © © & @ 


* ° ze ef Ld s 
APPA a oO OE eh (hyd eeu ear peat any ny Duet er ja So Fy nt Duet a? Sy far 2 J 2 DD sn DO 2D Se J 2D 2 2S 2 


(load "harmonic-equation.cl") 
(load "robot-kinematics.cl") 
(load "euler-angle-rigid-body.cl") 
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end be : strobe-camera.cl 

Seay We heletena : Dr. R. McGhee 

if : Naval Postgraduate School 
af : Monterey, CA 93943 


;; Summary : This file contains strobe-camera class definitions. 
KARR KKKRKKRK KKK KK KKK KEK KEKE KEKE KEK KR KK KEKE KEKE KKK KKK KKK KKK KK KKK KKK KKK KKK KKK 


(require :xcw) 
(Cw: initialize-common-windows) 


(defclass strobe-camera (rigid-body) 
((focal-length 
saccessor focal-length 
3 aLiglali@giepenta) (3) )) 
(camera-window 
:accessor camera-window 
:initform (cw:make-window-stream :borders 5 
:left 470 
Lereneiceyyl Sy0)(51 
‘width 650 
:height 650 
:title "Two Link Inverted Pendulum 
(Constant Length Massless Leg) 
LAGRANGIAN" 
:activate-p t)) 
(H-matrix 
z:initform (homogeneous-transform .3 -.3 0 -300 -90 -90)) 
(inverse-H-matrix 
:accessor inverse-H-matrix 
:initform (inverse-H (homogeneous-transform .3 -.3 0 -300 -90 -90))) 
(enlargement-factor 
:accessor enlargement-factor 
Jide worm 5 ON.) 


(defmethod move ((camera strobe-camera) azimuth elevation roll x y 2) 
(setf (H-matrix camera) (homogeneous-transform azimuth elevation roll x y 2)) 
(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera) ))) 


(defmethod take-picture ((camera strobe-camera) (body rigid-body) ) 
(let ((camera-space-node-list (mapcar #' (lambda (node-location) 
(post-multiply (inverse-H-matrix camera) node-location) ) 
(transformed-node-list body) ))) 
(dolist (polygon (polygon-list body) ) 
(clip-and-draw-polygon camera polygon camera~-space-node-list)))) 


(defmethod clip-and-draw-polygon 

((camera strobe-camera) polygon node-coord-list) 

(do* ((initial-point (nth (first polygon) node-coord-list) ) 
(from-point initial-point to-point) 
(remaining-nodes (rest polygon) (rest remaining-nodes) ) 
(to-point (nth (first remaining-nodes) node-coord-list) 

(if (not (null (first remaining-nodes) ) ) 
(nth (first remaining-nodes) node-coord-list)))) 

(null co-=point) 


102 


(draw-clipped-projection camera from-point initial-point) ) 
(draw-clipped-projection camera from-point to-point) )) 


(defmethod draw-clipped-projection ((camera strobe-camera) from-point to-point) 


(cond ((and (<= (first from-point) (focal-length camera) ) 
(<= (first to-point) (focal-length camera))) nil) 
((<= (first from-point) (focal-length camera) ) 


(draw-line-in-camera-window camera 
(perspective-transform camera (from-clip camera from-point to-point) ) 
(perspective-transform camera to-point) )) 
((<= (first to-point) (focal-length camera) ) 
(draw-line-in-camera-window camera 
(perspective-transform camera from-point) 
(perspective-transform camera (to-clip camera from-point to-point) ) )) 
(t (draw-line-in-camera-window camera 
(perspective-transform camera from-point) 
(perspective-transform camera to-point))))) 


(defmethod from-clip ((camera strobe-camera) from-point to-point) 


(Tet ((scale-factor (/ (- (focal-length camera) (first from-point) ) 
CmGrrest weo-point) (first. from—point) ) ))i) 
(list (+ (first from-point) 

(sseate-factror (- (first to-point) (first from=-point) ))) 

(+ (second from-point) 
(* scale-factor (- (second to-point) (second from-point) ) )) 

(+ (third from-point) 
(* scale-factor (- (third to-point) (third from-point)))) 1))) 

(defmethod to-clip ((camera strobe-camera) from-point to-point) 


(from-clip camera to-point from-point) ) 


(defmethod draw-line-in-camera-window ((camera strobe-camera) start end) 
(cw:draw-line (camera-window camera) 
(cw:make-position :x (+ 150 (first start) ) 
:y (+ 150 (second start) )) 
(cw:make-position :x (+ 150 (first end) ) 
7y (+ 150 (second end) )) 
:brush-width 0) ) 


(defmethod perspective-transform ((camera strobe-camera) point-in-camera-space) 


(let* ((enlargement-factor (enlargement-factor camera) ) 
(focal-length (focal-length camera) ) 
(x (first point-in-camera-space) ) ;X aXis is along optical axis 
(y (second point-in-camera-space)) ;y 1s out right side of camera 
(z (third point-in-camera-space))) ;z is out bottom of camera 
(list (+ (round (* enlargement-factor (/ (* focal-length y) x))) 

150) ;to right in camera window 

(+ 150 (round (* enlargement-factor (/ (* focal-length (- z)) x)) 


))))) yup in camera window 


(defun test-camera (z theta) ;Produces top view of default rigid-body. 
(setf robot-leg (make-instance 'rigid-body) ) 
(initialize robot-leg) 
(set-transformed-node-list-z robot-leg 2) 
(set-transformed-node-list-theta robot-leg theta) 
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(setf camera-1 (make-instance 'strobe-camera) ) 
(move camera-1l (deg-to-rad 0) (deg-to-rad 0) (deg-to-rad 0) -30 0 0) 
(take-picture camera-1 robot-leg) ) 


(defun deg-to-rad (angle) (* .01745329251994329 angle) ) 
(defun animation () 
(do ((theta-loop 0 (+ theta-loop 1))) 


((> theta-loop 90) ‘end) 
(test-camera 30 theta-loop))) 
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TWO LINK INVERTED PENDULUM SIMULATION WITH NEWTON-EULER 
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7; File >: euler-angle-rigid-body.cl 

>; Author : Dr. R. McGhee 

i; : Naval Postgraduate School 

ie : Monterey, CA 93943 

;; Modified by : Mehmet Bediz 

;; Summary : This file contains rigid body class which is implemented 
by Dr. R. McGhee. The function “draw-inverted-pendulum” 
and the other functions called by this 


function are implemented by Mehmet Bediz. 
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(defclass rigid-body 
() 
((posture ;The vector (xe ye ze phi theta psi). 
fintecorm, (0 0 0 0 0 0) 
“initaLg > posture 
:accessor posture) 
(posture-rate ;The vector (xe-dot ye-dot ze-dot phi-dot theta-dot psi-dot). 
:initarg :posture-rate 
:accessor posture-rate) 
(velocity ;The six-vector (uv wpqsr) in body coordinates. 
SEntrorm) (Os0 0000.0) 
:initarg :velocity 
:accessor velocity) 
(velocity-growth-rate ;The vector (u-dot v-dot w-dot p-dot q-dot r-dot). 
:accessor velocity-growth-rate) 
(forces-and-torques ;The vector (Fx Fy Fz L MN) in body coordinates. 
Pinterormn (list 0 0-60 0 0 0) 
:accessor forces-and-torques) 
(moments-of-inertia ;The vector (Ix Iy Iz) in principal axis coordinates. 
sii teremm (il LOO} 
:initarg :moments-of-inertia 
:accessor moments-of-inertia) 
(mass 
-initftorm 100 
:initarg :mass 
:accessor mass) 
(node-list ;(x y z 1) in body coord for each node. Starts with (0 0 0 1). 
inne ommeencomor OL) (-0.5 -0.5 0.5 1) (0.5 -0.5 0.5 1)(0.5 -0.5 -0.5 I) 
(055-0 oe ors ol) (—-02 590.5 02551) (0.5 0.5 0.5 1) 
(OES Osoe-Oe 5a) (05, 0. 55-025 2700 0 091) (0 0.3 1)) 
:initarg :node-list 
:accessor node-list) 
(polygon-list 
minmtmuowmem (i) 56 4)(5 67 8)(6 2 3 7)(2 1 4 3) (9 10)) 
wan ttarg -polygon-list 
:accessor polygon-list) 
(transformed-node-list ;(x y z 1) in earth coord for each node in node-list. 
Vimienemme(cn OO 1) 2 5-2 0 1) (2 -2 0 1) (2 -2 -30 1)(-2 -2 =30 1) 
(-—2 201) (2 201)(2 2 -30 1)(-2 2 =-30 1)) 
:accessor transformed-node-list) 
(H-matrix 
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-initform (uni &-matrisce2) 
:accessor H-matrix) 
(time-stamp 

:accessor time-stamp) ) ) 


(defmethod initialize ((body rigid-body) ) 
(setf (transformed-node-list body) (node-list body) ) 
(set£ (velocity-growth-rate body) (update-velocity-growth-rate body) ) 
(setf£ (posture-rate body) (earth-velocity body) ) 
(setf (time-stamp body) (get-internal-real-time) ) )) 


(defmethod set-transformed-node-list-z((body rigid-body) 2) 


(setf (third(fourth (transformed-node-list body))) (- z)) 
(setf£ (third(fifth (transformed-node-list body))) (- 2)) 
(setf (third(eighth (transformed-node-list body))) (- z)) 
(setf (third(ninth (transformed-node-list body))) (- z)) 


(transformed-node-list body) )) 


(defun transform (obj) 
(list 0 (first obj) (Second tobn Gent raycos) 


(defun reverse-transform (obj) 
(list (second obj) (third cba) (toumenvooa)eel) 


(defmethod move ((body rigid-body) azimuth elevation roll x y 2) 
(setf (posture body) (list x y z roll elevation azimuth) ) 
(setf£ (H-matrix body) 

(homogeneous-transform azimuth elevation roll x y 2Z)) 
(transform-node-list body) ) 


(defmethod get-delta-t ((body rigid-body)) 0.005) 


(defmethod forces-earth-to-body (Fx-e Fy-e Fz-e theta) 
(let* ((forces-earth (list Fx-e Fy-e Fz-e)) 


(R-matrix (rotation-matrix 0 (- theta) 0)) 
(forces-body (post-multiply R-matrix forces-earth) ) ) 


forces-body) ) 


(defmethod update-rigid-body ((body rigid-body) ) ;Euler integration. 
(let* ((delta-t (get-delta-t body) )) 

(update-posture body delta-t); EULER 

(update-velocity body delta-t); EULER 

(setf (H-matrix body) (homogeneous-transform (sixth (posture body) ) 
(fifth (posture body)) (fourth (posture body)) (first (posture body) ) 
(second (posture body)) (third (posture body) ))) 

(transform-node-list body) 

(update-velocity-growth-rate body) )) 


(defmethod update-velocity-growth-rate ((body rigid-body) ) 
(setf (velocity-growth-rate body) ;Assumes principal axis coordinates with 


(multiple-value-bind -origin at center cf gravity Cf body. 
(Fx Fy Fz L MN ll. VeWwep cis Ix Iy Iz) ;Declares local variables. 
(values-list ;Values assigned. 
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(append 
(forces-and-torques body) (velocity body) (moments-of-inertia body) ) ) 
(list (+ (* v r) (* -1 wq) (/ Fx (mass body) ) 
CVaseavity (first (third (H-matrix body)))))- 
fe 1 wep) (~ -1 ur) (7 Fy (mass body) ) 
(* *gravity* (second (third (H-matrix body) )))) 
ree) t= ever, vz (mass body) ) 
(* *“Gravity~ @@@eard (third (H-matrix body))))) 


(Cee = hy La )oecp re) a) 1x) 
TC tae ee) ee) Mi) Ly) 
(J+ (* ees Ty) Gg) N) 1z))))) =:Value returned. 


(defmethod update-velocity ((body rigid-body) delta-t) ;Euler integration. 
(setf£ (velocity body) 
(vector-add (velocity body) 
(scalar-multiply delta-t (velocity-growth-rate body))))) 


(defmethod update-posture ((body rigid-body) delta-t) ;Euler integration. 
(seti (posture-rate body) (earth-velocity body) ) 
(setf£f (posture body) 
(vector-add (posture body) (scalar-multiply delta-t (posture-rate body))))) 


(defmethod transform-node-list ((body rigid-body) ) 
(setf£ (transformed-node-list body) 
(mapcar #' (lambda (node-location) 
(post-multiply (H-matrix body) node-location) ) 
(node-list body) ))) 


idetconstant “~gravity* 32.2185) 


(defmethod earth-velocity ((body rigid-body) ) 
(let* ((linear-velocity (firstn 3 (velocity body) )) 
(rotational-velocity (cdddr (velocity body) )) 
(posture (posture body) ) 
(R-matrix (rotation-matrix (sixth posture) (fifth posture) 
(fourth posture) )) 
(linear-earth-velocity (post-multiply R-matrix linear-velocity) ) 
(T-matrix (body-rate-to-euler-rate-matrix (sixth posture) 
(fifth posture) (fourth posture) )) 
(rotational-earth-velocity (post-multiply T-matrix 
rotational-velocity) )) 
(append linear-earth-velocity rotational-earth-velocity) ) ) 


(defun test-rigid-body () 
(setf airplane-1 (make-instance ‘rigid-body) ) 
(initialize airplane-1) 
(setf camera-1 (make-instance 'strobe-camera) ) 
(move camera-1l (- (/ pi 2)) 0 00 30 0 ) 
(take-picture camera-1 airplane-1) 
(dotimes (i 20 'done) (update-rigid-body airplane-1)) 
(take-picture camera-1 airplane-1) ) 


107 


eo enunrewecuesteegtee en eeseea s+ a2 ee 6 @ 6 60 he ee Selle helhlUwthlUelUl8UlUlel le lhl le US le! 8) le le ‘os 6) ee ie iw 6) s) 6) 6) 66 ei) © ee) eee hee ile ellen al et el er elle i eres 


| a a J Se i J A ee Jae ee ee fe Se et Ct et ee De) et ee et Aa et pet et Pat Moet Pet at fh PP AP VP We CR PP ER TE TR PRN TR OS) RD PPX PO TP PE TP fp Po) 


two Link Inverted Pendulum 
Massless leg and a single rigid body (fixed leg length) 
State vector x = (theta-1 theta-l-dot theta-2 theta-2-dot) 
state vector u = (M-1 M-2) 

- control torque at the ankle (M-1) 

- control torque at the hip (M-2) 
Newton-Euler Method 

- 3x3 matrix inversion to calculate forces and moments 
Heun Integration 


ttt éte tert te ee ee ees ae ae aa eae area eee ee) ear ere re ae PP Oe Pe ee PT tt ee ee i 


(set£ 1 
(setf rx 


025) 
37) 


(setf time-step 0.01) 


(defun solve-three-equation-system (state-vector K-theta-1 K-theta-l-dot 


(setf£t 
(sett 


(sett 


K-theta-2 K-theta-2-dot) 
100) 
025) 
3 
100) 


(sett 
(set£ 
(sett 


(set£ 
(sett 


m 
ak 
(setf xr 
iL 
M- 


1 (compute-M-1 state-vector K-theta-1 K-theta-l-dot 
K-theta-2 K-theta-2-dot) ) 

M-2 (compute-M-2 state-vector K-theta-l K-theta-1l-dot 

K-theta-2 K-theta-2-dot) ) 


theta-1l (first state-vector) ) 
theta-l-dot (second state-vector) ) 
theta-2 (third state-vector) ) 


(setf theta-2-dot (fourth state-vector) ) 
3222) 


(setf g 


(post-multiply 
(matrix-inverse 
(lise 


(list 


i 


(+ 


(+ 


(rst aeo JE (* -1l 1 (Sin (- theta-2 theta-1)))) 
(list (* m “rf (cos theta-1)) (* m 1 (cos theta-2)) 


(* -1 (sin theta-1) )) 


(list (* m xr (sin theta-l1)) (* m 1 (sin theta-2)) 


(cos theta-1)))) 


(* (- (* M-2 (+ 1 4r)) (* 1 M-1)) (cos (- theta-2 theta-1))) r) 
(* mr theta-l-dot theta-l-dot (sin theta-1) ) 

(* m 1 theta-2-dot theta-2-dot (sin theta-2) ) 

(/ (* (- M-1 M-2) (cos theta-1)) r)) 

(* -1l mr theta-l-dot theta-l-dot (cos theta-1)) 

(* -1l m 1 theta-2-dot theta-2-dot (cos theta-2)) 

(2 tes,) 

(/ (* (- M-1 M-2) (sin theta-1)) 4r))))) 


(defun euler-step-constant-leg (state-vector threeXthree-matrix) 
(mapcar ‘+ state-vector (scalar-multiply time-step 


(derivative-state-vector 
state-vector threeXthree-matrix) ))) 
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(defun heun-step-constant-leg (x K-theta-1 K-theta-l-dot 
K-theta-2 K-theta-2-dot) 
(setf threeXthree-matrix (solve-three-equation-system 
x K-theta-1 K-theta-l-dot 
K-theta-2 K-theta-2-dot) ) 
(mapcar '+ x (scalar-multiply (* time-step .5) 
(vector-add (derivative-state-vector x threeXthree-matrix) 
(derivative-state-vector 
(euler-step-constant-leg x threeXthree-matrix) 
threexthree-matrix) )))) 


(defun derivative-state-vector (state-vector threeXthree-matrix) 
(list (second state-vector) 
(first threeXthree-matrix) 
(fourth state-vector) 
(second threeXthree-matrix) ) ) 


(defun compute-M-1l1 (state-vector K-theta-1 K-theta-l-dot 
K-theta-2 K-theta-2-dot) 
(setf theta-1l (first state-vector) ) 
(setf theta-l-dot (second state-vector) ) 
(+ (* -1 K-theta-1 theta-1) (* -1 K-theta-l-dot theta-1l-dot) )) 


(defun compute-M-2 (state-vector K-theta-1 K-theta-l-dot 
K-theta-2 K-theta-2-dot) 
(setf theta-2 (third state-vector) ) 
(setf theta-2-dot (fourth state-vector) ) 
(+ (* -1l K-theta-2 theta-2) (* -1 K-theta-2-dot theta-2-dot) )) 


(defun compute-y (state-vector) 


(setf theta-1 (first state-vector) ) 
(setf theta-l-dot (second state-vector) ) 
(setf theta-2 (third state-vector) ) 
(SEL Feneta—-Z-coet (fourth state-vector) ) 
(setf 1 G25) 

(setf xr 3) 


(+ (* xr (sin theta-1) ) 
(~ 1) ($in theta-2)))) 


(defun compute-z (state-vector) 


(setf theta-1l (first state-vector) ) 
(setf theta-l-dot (second state-vector) ) 
(setf theta-2 (third state-vector) ) 
(setf theta-2-dot (fourth state-vector) ) 
(setf 1 C25) 

(setf xr 3) 


(+ (* xr (cos theta-1)) 
(* 1 (cos theta-2)))) 


(defun draw-inverted-pendulum (halt-time K-theta-1 K-theta-1l-dot 
K-theta-2 K-theta-2-dot) 


(setf robot-body (make-instance ‘rigid-body) ) 
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(initialize ~robot—boa 
(setf robot-leg (make-instance ‘rigid-body) ) 
(initialize robot-leg) 


(set£ camera~-l (make-instance 'strobe-camera) ) 


(move camera-1l 


(setf graph-l-window (make-window-graph-1) ) 
(draw-coordinate-axes graph-1-window) 


(setf graph-2-window (make-window-graph-2) ) 
(draw-coordinate-axes graph-2-window) 


(deg-to-rad 0) (deg-to-rad 0) (deg-to-rad 0) 


=O 00) 


(do ((x '(0.5 01.0 0) (heun-step-constant-leg x K-theta-1 K-theta-l-dot 
K-theta-2 K-theta-2-dot) ) 
(old-—xv1s(0e5- 0) L030 ae) 
(time 0 (+ time 1)» 
((> time halt-time) ‘'done) 
(move robot-body (deg-to-rad 0) (deg-to-rad 0) (third x) 
0 (compute-y x) (* -l(compute-z x))) 
(setf£ (transformed-node-list robot-leg) (list (list 0 0 0 1) 
(last 0 (* xr (sin (£irSt eee 
(* <1 ¥ (cos (£irst 2) 
(TISts0. 00 r ya) 
(setf (polygon-list robot-leg) (list (list 2 1))) 
(cw:clear (camera-window camera-1) ) 
(take-picture camera-1l robot-body) 
(take-picture camera-l robot-leg) 
(draw-line-in-window graph-l-window 80 (list (* time-step (- time 1)) 
(first cid) 
(list (* time-step time) 
(firsts. 
(draw-line-in-window graph-2-window 80 (list (* time-step (- time 1)) 
(thirdtadd-=x)) 
(list (* time-step time) 
(third Sep) ) } 
(defun make-window-graph-1 () 
(cw:make-window-stream :borders 5 
:left 0 
*bottom 50 
:width 450 
*herghe 450 
:title "Leg Angle (theta-1) NEWTON-EULER" 


:activate-p t)) 
(defun make-window-graph-2 () 


(cw:make-window-stream :borders 5 
‘left 0 
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:Make window visible. 


“Bottom 550 


:width 450 
sheight 450 
:title "Body Attitude (theta-2) NEWTON-EULER" 


:activate-p t)) ;Make window visible. 


el 
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Ae dash IS : load-euler-files.cl 

; 7 -Awthor : Mehmet Bediz 

ae : Naval Postgraduate School 

ae : Monterey, CA 93943 

;; Summary : This file contains functions. ton "oad. 21 lec 
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; Load files 
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(load "harmonic-equation.cl") 
(load "robot-kinematics.cl") 

(load "euler-angle-rigid-body.cl") 
(load "strobe-camera.cl") 


a 
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77 File ; Strobe-camera.cl 

peaeeuchor > Dr. R. McGhee 

ii : Naval Postgraduate School 

77 : Monterey, CA 93943 

77 Summary : This file contains strobe-camera class definitions. 


me kk keke Ke KKK KKK KKK KK KKK KKK KK KKK KKK KKK KKK RK KKK KKK KKK KKK KKK KKK KK KKK KKK KKK 
(require :xcw) 
(cw: initialize-common-windows) 


(defclass strobe-camera (rigid-body) 
((focal-length 
saccessor focal-length 
ment LOLM 6) 
(camera-window 
saccessor camera-window 
:initform (cw:make-window-stream sporders 5 
:left 470 
soot tom..oU0 
:width 650 
“nelgne 620 
:title "Two Link Inverted Pendulum 
(Constant Length Massless Leg) 
NEWTON-EULER"” 
sactivate-p t)) 
(Ho=matrix 
sinitform (homogeneous-transform .3 -.3 0 -~300 -90 -90)) 
(inverse-H-matrix 
saccessor inverse-H-matrix 
Pee rormuetinverse-H {(homogeneous-transform .3 -.3 0 -300 -90 -90))) 
(enlargement-factor 
saccessor enlargement-factor 
amie rorm 20) ) ) 


(defmethod move ((camera strobe-camera) azimuth elevation roll x y 2) 
(setf (H-matrix camera) (homogeneous-transform azimuth elevation roll x y z)) 
(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera) ))) 


(defmethod take-picture ((camera strobe-camera) (body rigid-body) ) 
(let ((camera-space-node-list (mapcar #' (lambda (node-location) 
(post-multiply (inverse-H-matrix camera) node-location) ) 
(transformed-node-list body)))) 
(aolist (polygon (polygon-list body) ) 
(clip-and-draw-polygon camera polygon camera-Space~node-list)))) 


(defmethod clip-and-draw-polygon 
((camera strobe-camera) polygon node-coord-list) 
iieaeetrneclal=point (nth (first polygon) node-coord-1list) ) 
(feom- pOlnelliitdel—point to-point) 
(remaining-nodes (rest polygon) (rest remaining~nodes) ) 
(tO=point (nth (first remaining-nodes) node-coord-list) 
(if (not (null (first remaining-nodes) ) ) 
(nth (first remaining-nodes) node-coord-list)))) 
GimuilscO-polnt) 
‘ (draw-clipped-projection camera from-point initial-point) ) 


(draw-clipped-projection camera from-point to-point) )) 


(defmethod draw-clipped-projection ((camera strobe-camera) from-point to-point) 
(cond ((and (<= (first from-point) (focal-length camera) ) 
(<= (first to-point) (focal-length camera))) nil) 
((<= (first from-point) (focal-length camera) ) 
(draw-line-in-camera-window camera 
(perspective-transform camera (from-clip camera from-point to-point) ) 
(perspective-transform camera to-point) )) 
((<= (first to-point) (focal-length camera) ) 
(draw-line-in-camera-window camera 
(perspective-transform camera from-point) 
(perspective-transform camera (to-clip camera from-point to-point)))) 
(t (draw-line-in-camera-window camera 
(perspective-transform camera from-point) 
(perspective-transform camera to-point))))) 


(defmethod from-clip ((camera strobe-camera) from-point to-point) 
(let ((scale-factor (/ (- (focal-length camera) (first from-point) ) 
(= (£irse to-point) (first from-peine ye 
(last: (+ (first £rom—poineg, 


(* scale-factor (— (Eirst to-point) (first from—poine) ow 
(+ (second from-point) 
(* scale-factor (- (second to-point) (second from-point) ))) 
(+ (third from-point) 
(* scale-factor (- (third to-point) (third from—point)) )) eee 


(defmethod to-clip ((camera strobe-camera) from-point to-point) 
(from-clip camera to-point from-point) ) 


(defmethod draw-line-in-camera-window ((camera strobe-camera) start end) 
(cw:draw-line (camera-window camera) 
(cw:make-position :x (+ 150 (first start) ) 
sy (+ 150 (second start) )) 
(cw:make-position :x (+ 150 (first end) ) 
:y (+ 150 (second end) )) 
:brush-width 0)) 


(defmethod perspective-transform ((camera strobe-camera) point-in-camera-space) 


(let* ((enlargement-factor (enlargement-factor camera) ) 

(focal-length (focal-length camera) ) 

(x (first point-in-camera-space)) ;x axis is along optical axis 

(y (second point-in-camera-space)) ;y is out right side of camera 

(z (third point-in-camera-space))) ;2z is out bottom of camera 

(list (+ (round (* enlargement-factor (/ (* focal-length y) x))) 
156) sto right in camera wince” 

(+ 150 (round (* enlargement-factor (/ (* focal-length (- z)) x)) 


1p i a, ;up in camera window 


(defun test-camera (z theta) ;Produces top view of default rigid-body. 
(setf robot-leg (make-instance ‘rigid-body) ) 
(initialize robot-leg) 
(set-transformed-node-list-z robot-leg 2z) 
(set-transformed-node-list-theta robot-leg theta) 
(setf camera-l (make-instance 'strobe-camera) ) 
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(move camera-1l (deg-to-rad 0) (deg-to-rad 0) (deg-to-rad 0) 


(take-picture camera-1 robot-leg) ) 
(defun deg-to-rad (angle) (* .01745329251994329 angle) ) 
(defun animation () 
(do ((theta-loop 0 (+ theta-loop 1))) 


((> theta-loop 90) ‘end) 
(test-camera 30 theta-loop) )) 


NS: 


-3 05900) 
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APPENDIX B: KINEMATIC SIMULATION SOFTWARE 


If 2K RR 2k 2 2 2k 2 2 2 Ae 2K 2 2k 2c 2 oie oe 2k a 2 2k 2k 26 2 2K 2k 6 2 2k a 2k ok 2k 2k 2c 2c oi 2k 2 2 2k 2k 2k kc 2k 2k 2k 2k 2 2k 2k 2K 2 2k 2 2k ok 2 a 2k ok ok 2k ok ok ok ok 


// File > main.C 


// Author : Mehmet Bediz 

// : Naval Postgraduate School 

// : Monterey, CA 93943 

// Created : November 1996 

// Summary : This file contains the main function and three motion 

Hf : functions of the kinematic model Dynaman: forward stepping, 
// . upward stepping, and jumping. 


I/ 3K 2K 2 2 2c 2 2c 2c 26 9k 2K 2 2K 216 oC 2k 2k 2K 2 a6 ak 2c 2 6 ok 26 2K 2k 2 2k kc 2k 2 2c 2 2 2k 2c oie kc ok 3 2c ok kc 26 2k 2s 2 2k oie kc 2k 2c 2 kc 296 2 2 2 2 ie 2 2 2 oe 2k ok 


#include <string.h> 

#include “main.h" 

void step_forward(int number_of_steps, float X, float Y, float Z); 
void step_upward(int number_of_steps, float X, float Y, float Z); 
void jump(float X,float Y,float Z); 


// Speed of the animation can be controlled by changing delta_t 
fitoat delta t= 2 * 3.0; 


pfNode *root; 

pfDCS =dcs: 

pfMatrix mat, orbit; 

pfSphere sphere; 

pfNode *modell, *model2, *model3, *model4; 
pfNode *model8, *model10; 

pfNode *modell1, *modell2, *model13, *model14; 
pfDCS *nodell, *nodel2, *nodel3, *nodel4; 
pfDCS *nodel, *node2, *node3, *node4, 
pfDCS *node8, *nodel0; 

pfDCS *dcs0,; 

pfDCS Baesie =desz, *dcs3: 

pies *dcs4, *dcs5, *dcs6; 

pfDCS *des7aacese, *des9, *dcs10; 

pfDCS SGcslumeeacsuz, *dcs|3: 

pfDCS sdesia. -cdcsl5, ~dcs16; 

pfDCS *des30, *des31, *dcs32, *dcs33; 

pfDCS Sdcssa2acs35, *dcs36: 

pfDCS Sdess7—dcs36, *dcs39; 

char Pfiileieehle2, *file3, *filed: 

char *file8, *file10; 

char *filell, *file12, *filel3, *file14; 
pfLightSource *light; 

pfMatrix lightMat; 

float ‘ale 

pfTexture Sex: 

pfFrustum 7 ETust: 

void *arena; 

pfDCS *lightDCS; 
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int 1; 


float final_position; 
| [--------~- 2-7-2 2nn nnn nnn nnn nana nnn nen nnn nnn nnn nn nen en neee en neee 
// Function: main 
// Returns: None 
// Parameters: None 
// Summary: Initializes IRIS Performer!™, loads body parts from poly format 
// files to DCS nodes, creates the channels at the upper right 
// corner of the window, calls step_forward, step_upward, and 
// jump functions with the number of steps and the initial 
I/ positions. 
| ]----------2----2-nn nanan nnn nn nn nena nnn en nn nn neem nnn n nn nn nnn anne renee 
void 


main (int argc, char *argv[]) 


{ 


/* choose default objects of none specified */ 
filel = “lowerleg.poly"; 
file2= "upperleg.poly”; 

files —— Headay | 

filed = "torso.poly"; 

file8 = "foot.poly"; 

filelO = "floor.iv"; 

filel1 = "rightupperarm.poly"; 
filel2 = "lowerarm.poly"; 
filel3 = "hand.poly”; 

filel4 = "leftupperarm.poly"; 


if (! stremp(argv[1],"slow")){ 
delta t= 2540.5: 
} 


#ifndef IRISGL 
printf("Sorry, shadows doesn't work in OPENGL\n"); 
return 0; 

#endif 


/* Initialize Performer */ 
pfinit(); 


/* Use default multiprocessing mode based on number of 
* processors. 

sat 

pfMultiprocess(PFMP_DEFAULT); 


/** allocate shared memory **/ 
InitShared(); 


/* Configure multiprocessing mode and start parallel 
* processes. 

zy 

pfConfig(); 
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/** Initialize Performer utility and GUI functions **/ 
pfulnitUtild; 


pflnitClock(0.0f); 
Shared->simTime = pfGetTime(); 
srand(Shared->simTime* 10000000); 


/* Append to PFPATH additional standard directories where 

* geometry and textures exist 

=] 
pfFilePath(".:/usr/share/Performer/data:/tmp_mnt/workb/bediz/Performer/data"); 


/* Do not use FLAT_ primitives because they look bad 

* with local lighting. 

“afl 

pfdBldrMode(PFDBLDR_MESH_LOCAL_LIGHTING, 1); 


/* Read a single file, of any known type. */ 
if ((root = pfdLoadFile("text.iv")) == NULL) 
{ 

pfExitQ); 

exit(-1); 


} 


/* Load the files */ 
if ((modell = pfdLoadFile(filel)) == NULL) 


{ 


pfExit(); 

exit(-1); 
} 
if ((model2 = pfdLoadFile(file2)) == NULL) 
{ 

pfExitQ); 

exit(-1); 
} 
if ((model3 = pfdLoadFile(file3)) == NULL) 
{ 

pfExitQ); 

exit(-1); 


} 
if ((model4 = pfdLoadFile(file4)) == NULL) 
{ 

pfExit(); 

exit(-1); 
} 
if ((model8 = pfdLoadFile(file8)) == NULL) 
{ 

pfExit(); 

exit(-1); 
} 
if ((model10 = pfdLoadFile(file10)) == NULL) 
{ 

pfExit(); 

exit(-1); 


Me 


if ((model] 1 = pfdLoadFile(filel1)) == NULL) 
{ 

pfExit(); 

exit(-1); 
} 
if ((model12 = pfdLoadFile(file]2)) == NULL) 
{ 

pfExit(); 

exit(-1); 
} 
if ((model13 = pfdLoadFile(file13)) == NULL) 
{ 

pfExit(); 

exit(-1); 


} 
if ((modell 4 = pfdLoadFile(file]4)) == NULL) 
{ 
pfExit(); 
exit(-1); 
} 


/* Create and attach loaded subgraph to a pfScene. */ 
Shared->scene = new pfScene; 

dcs = new pfDCS; 

dcs->addChild(root); 


Shared->scene->addChild(dcs); 
/* determine extent of scene's geometry */ 
Shared->scene->getBound(&(Shared->bsphere)); 


[7 6 2 28 3 28 A 2 AE HH 2 Kk Ok 2 HK MAIN WINDOW Oi OG 2k OK 2k a ea RE 9 OE OE / 


/* Configure and open GL window */ 

Shared->p = pfGetPipe(0); 

Shared->p2 = pfGetPipe(0); 

Shared->pw = new pfPipeWindow(Shared->p); 
Shared->pw->setName("DYNAMAN"); 
Shared->pw->setConfigFunc(OpenPipe Win); 
Shared->pw->setOriginSize(120, 120, 1100, 1 100); 
Shared->pw->config(); 


/* initializes mouse and keyboard inputs to be read from window */ 
pfulnitInput(Shared->pw, PFUINPUT_GL); 
pfulnitGUI(Shared->pw); 

pfuEnableGUI(TRUE); 


/* Create and configure a pfChannel. */ 
Shared->chan = new pfChannel(Shared->p); 
Shared->chan->setScene(Shared->scene); 
Shared->chan->setNearFar(1.0f, 5.0f * Shared->bsphere.radius); 
Shared->chan->setFOV(45.0f, 0.0f); 
Shared->view.xyz.set(1.3f * Shared->bsphere.radius, 

-2.5f * Shared->bsphere.radius, 

3.0f * Shared->bsphere.radius); 
Shared->view.hpr.set(0.0f, -45.0f, 0.0f); 
Shared->chan->set View(Shared->view.xyz, Shared->view.hpr); 
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Shared->chan->setTravFunc(PFTRA V_DRAW,DrawChannel); 


[RRR RR KK FIRST CHANNEL 2 ee ee ee 2 ae 2 ee A ae oR 2 a ae / 
Shared->chan2[0] = new pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[0]); 
Shared->chan2(0]->setTravFunc(PFTRA V_DRAW,DrawChannel); 
Shared->chan2[0]->setScene(Shared->scene); 
Shared->chan2(0]->setNearFar(1.0f, 5.0f * Shared->bsphere.radius); 
Shared->chan2[0]->setFOV(45.0f, 0.0f); 
Shared->view.xyz.set( Shared->bsphere. radius, 

-4.0f * Shared->bsphere.radius, 

0.8f * Shared->bsphere.radius); 
Shared->view.hpr.set(0.0f, 0.0f, 0.0f); 
Shared->chan2[0]->set View(Shared->view.xyz, Shared->view.hpr); 


[EPR ERA KE SECOND CHANNEL Oe 26 A Se ee ee 2 ee 2 oe eo 2 ae 2 2 2 KK Kf 


Shared->chan2[1] = new pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[1]); 
Shared->chan2[1]->setTravFunc(PFTRA V_DRA W,DrawChannel); 
Shared->chan2[1]->setScene(Shared->scene); 
Shared->chan2[1]->setNearFar(1.Of, 5.0f * Shared->bsphere.radius); 
Shared->chan2[1]->setFOV(45.0f, 0.0f); 
Shared->view.xyz.set(1.3_ * Shared->bsphere.radius, 

-2.5f * Shared->bsphere.radius, 

3.0f * Shared->bsphere.radius); 
Shared->view.hpr.set(0.0f, -45.0f, 0.0f); 
Shared->chan2[1]->set View(Shared->view.xyz, Shared->view.hpr); 


re ox THIRD CHANNEL ee ee ee te) 


Shared->chan2[2] = new pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[2]); 
Shared->chan2[2]->setTravFunc(PFTRAV_DRAW,DrawChannel): 
Shared->chan2[2]->setScene(Shared->scene); 
Shared->chan2[2}->setNearFar(1.Of, 5.0f * Shared->bsphere.radius); 
Shared->chan2[2]->setFOV(45.07, 0.0f); 
Shared->view.xyz.set( 1.6 * Shared->bsphere.radius, 

-0.15f * Shared->bsphere.radius, 

2.5f * Shared->bsphere.radius); 
Shared->view hpr.set(0.0f, -90.0f, 0.0f); 
Shared->chan2[2]->set View(Shared->view.xyz,Shared-> view.hpr); 


Db Se Se FOURTH CHANNEL 2k ek a 2 ke A a 2 2 a 2 a a 2 2 a ak 2 ok / 


Shared->chan2[3] = new pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[3]); 
Shared->chan2[3]->setTravFunc(PFTRA V_DRAW,DrawChannel); 
Shared->chan2[3]->setScene(Shared->scene); 
Shared->chan2[3]->setNearFar(1.0f, 5.0f * Shared->bsphere.radius); 
Shared->chan2[3]->setFOV(45.0f, 0.0f); 
Shared->view.xyz.set( 4.0f * Shared->bsphere.radius, 

-0.5f * Shared->bsphere.radius, 

0.5 * Shared->bsphere.radius); 

Shared->view.hpr.set(90.0f, 0.0f, 0.0f); 


Shared->chan2[3]->set View(Shared->view.xyz, Shared->view.hpr); 
Shared->chan2[3]->set Viewport(0.7, 0.85, 0.7, 0.85); 
Shared->chan2[2]->set Viewport(0.85, 1.0, 0.7, 0.85); 
Shared->chan2[1]->set Viewport(0.85, 1.0, 0.85, 1.0); 
Shared->chan2[0]->set Viewport(0.7, 0.85, 0.85, 1.0); 


orbit.makeRot(1.0f, 0.0f, 0.0f, 1.0f); 


/* scale models to unit size */ 
node] = new p{DCS; 
nodel ->addChild(model1 ); 
modell ->getBound(&sphere); 
if (sphere.radius > 0.0f) 
nodel ->setScale(1.Of/sphere.radius); 


node2 = new pfDCS; 

node2->addChild(model2); 

model2->getBound(&sphere); 

if (sphere.radius > 0.0f) 
node2->setScale(1.0f/sphere.radius); 


node3 = new pfDCS; 

node3->addChild(model3); 

model2->getBound(&sphere); 

if (sphere.radius > 0.0f) 
node3->setScale(1.0f/sphere.radius); 


node4 = new pfDCS; 

node4->addChild(model4); 

model4->getBound(&sphere); 

if (sphere.radius > 0.0f) 
node4->setScale(1.0f/sphere.radius); 


node8 = new pfDCS; 

node8->addChild(model8); 

model8->getBound(&sphere); 

if (sphere.radius > 0.0f) 
node8->setScale(1.0f/sphere.radius); 


node10 = new pfDCS; 

node10->addChild(model10); 

model] 0->getBound(&sphere); 

if (sphere.radius > 0.0f) 
node10->setScale(1.0f/sphere.radius); 


nodell = new pfDCS; 

nodel 1 ->addChild(model1 1); 

model! 1->getBound(&sphere); 

if (sphere.radius > 0.0f) 
node10->setScale(1.0f/sphere.radius); 


node12 = new pfDCS; 
nodel2->addChild(model1 2), 
modell 2->getBound(&sphere); 
if (sphere.radius > 0.0f) 


hee 


node 10->setScale(1.0f/sphere.radius); 


node13 = new pfDCS; 

node 13->addChild(model] 3); 

model 13->getBound(&sphere); 

if (sphere.radius > 0.0f) 
node10->setScale(1.0f/sphere.radius); 


node1l4 = new pfDCS; 

node 14->addChild(model14); 

model14->getBound(&sphere); 

if (sphere.radius > 0.0f) 
node10->setScale(1.Of/sphere.radius); 


/* Lighting without shadowing */ 
// put a default light source in the scene 
Shared->scene->addChild(new pfLightSource); 


arena = pfGetSharedArena(); 


// Create and configure shadow frustum. Fit frustum tightly to scene. 
Frust = new(arena) pfFrustum; 
Frust->makeSimple(FOV); 


d = Shared->bsphere.radius / sinf(PF_DEG2RAD(FOV/2.0f)); 
d = PF_MAX2(d, NEAR + Shared->bsphere.radius); 


Frust->setNearFar(NEAR, d + 1.1f * Shared->bsphere.radius); 
tex = initSpotTex(); 


light = new pfLightSource; 
light->setMode(PFLS_PROJTEX_ENABLE, 1); 
light->setAttr(PFLS_PROJ_FRUST, Frust); 
light->setAttr(PFLS_PROJ_TEX, tex); 
light->setColor(PFLT_DIFFUSE, 1.0f, 1.0f, 1.0f); 
light->set Val(PFLS_INTENSITY, 3.0f); 
light->setPos(17.0 , -7.0, 13.9, i1.0f);// Make light iocal 
light->setSpotDir(0.0f, 0.0f, -10.0f); 


/! Make DCS to move lights around 
lightDCS = new pfDCS; 
lightDCS->addChild(light); 


/* MainDCS */ 
dcsO = new pfDCS; 


/* Left Foot */ 
des] = new pfDCS; 
dcsl -> addChild(nodel); 


/* Left lowerleg */ 
/* Right lowerleg */ 


dcs4 = new pfDCS; 
dcs4 -> addChild(node]l ); 


/ Teett upperles: 
dcs2 = new pfDCS; 
des2 -> addChild(node2); 


/* Right upperleg */ 
dcs5 = new pfDCS; 
des5 -> addChild(node2); 


/* Floor */ 

dcs10 = new pfDCS; 

dcs10 -> addChild(node10); 
dcs30 = new pfDCS; 

dcs30 -> addChild(node10); 
dcs31 = new pfDCS; 

dcs31 -> addChild(node10); 
dcs32 = new pfDCS; 

dcs32 -> addChild(node1 0); 
dcs33 = new pfDCS; 

dces33 -> addChild(node10); 
dcs34 = new pfDCS; 

dcs34 -> addChild(node1 0); 
dcs35 = new pfDCS; 

dcs35 -> addChild(node10); 
dcs36 = new pfDCS; 

dcs36 -> addChild(nodel0); 
dcs37 = new pfDCS; 

dcs37 -> addChild(node10); 
dcs38 = new pfDCS; 

dcs38 -> addChild(nodel 0); 
des39 = new pfDCS; 

dcs39 -> addChild(node10); 


/* Torso (Two parts) */ 
desG = new piDGs; 

dcs6 -> addChild(node4); 
dcsO -> addChild(dcs6); 


dcs3 = new pfDCS; 
dcs3 -> addChild(node4); 
dcsO -> addChild(dcs3); 


i Teett veos: 
dcs2 -> addChild(dcs1); 
dcsO -> addChild(dcs2); 


/* Right Legs */ 
dcs5 -> addChild(dcs4); 
dcsO -> addChild(dcs5); 
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dcs8 = new pfDCS; 

dcs8 -> addChild(node8); 
dcs9 = new pfDCS; 

dcs9 -> addChild(node8); 


desl -> addChild(dcs8); 
des4 -> addChild(dcs9); 


Head © */ 

des? =new piDCs: 

dcs7 -> addChild(node3); 
dces3 -> addChild(dcs7); 


/* Rightupper arm */ 
dcs11 = new pfDCS; 

dcs11 -> addChild(node1 1); 
desO -> addChild(dces11); 


/* Right lower arm */ 
dcs12 = new pfDCS; 

dces12 -> addChild(node1 2); 
des11 -> addChild(dcs12); 


/* Righthand */ 

dcs13 = new pfDCS; 

des13 -> addChild(node13); 
des12 -> addChild(dcs13); 


(Pelt upper arm */ 
dcs14 = new pfDCS; 

dces14 -> addChild(node 14); 
desO -> addChild(dcs 14); 


/* Leftlower arm */ 
des15 = new pf{DCS; 

dces15 -> addChild(nodel 2); 
dces14 -> addChild(dcs15); 


se eetthand */ 

dcs16 = new pfDCS; 

dcs16 -> addChild(node 13); 
dcesi5 -> addChild(dcs16); 


// Stairs 

dces30 -> addChild(dcs31); 
des31 -> addChild(dcs32); 
dces32 -> addChild(dcs33); 
dces33 -> addChild(dcs34); 
dcs34 -> addChild(dcs35); 
dcs35 -> addChild(dcs36); 
dcs36 -> addChild(dcs37); 
dcs37 -> addChild(dcs38); 
dcs38 -> addChild(dcs39); 


Shared->scene-> addChild(dcs0Q); 
Shared->scene-> addChild(dcs30); 


/* Create "floor letters" */ 
des -> addChild(initFloor(&Shared->bsphere)); 
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/* Floor i) 
dcs10->setScale(2.0f); 
dcs10->setRot(0.0, -90.0 ,0.0); 
des10->setTrans(-1.0 ,-11.0 , 0.05); 


// Draw steps 
dcs30->setScale(0.5f); 
dcs30->setRot(-90.0, -90.0 ,0.0); 
dcs30->setTrans(13.0 , 0.0 , 0.05); 
dcs31->setTrans(0.0 , -2.0 , 5.4); 
dces32->setIrans(0.0 , -2.0 , 4.1); 
des33->setTrans(0.0 ,-1.9, 4.1); 
dcs34->setTrans(0.0 ,-1.9 , 4.1); 
des35->setTrans(0.0 , -1.9, 4.1); 
dces36->setTrans(0.0 , -1.8 , 4.1); 
dces37->setTrans(0.0 , -1.8, 4.1); 
dcs38->setTrans(0.0 , -1.8, 4.1); 
dcs39->setTrans(0.0 ,-1.7, 4.1); 


/* Set up initial/default view */ 
MakeGUI(); 
int temp = system("sfplay runaway.wav &"); 
if (-1 == temp){ 

printf("Can't play \n"); 


y 
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else{ 

printf("Playing %i \n",temp); 

} 

for (int forever = 0; forever < 50; forever ++){ 
step_forward(6, -9.8 , -7.0, 3.9); 
step_upward(5, 13.3 , -7.0 , 3.9); 
jump(31.7, -7.0,12.496556); 
step_forward(4, 39.137990 , -7.0 , 4.670563); 

\ 

J 

// Kill music 

system("music_killer”); 


/* Terminate parallel processes and exit. */ 
pfExit(); 
\ 


J 


// Function: step_forward 

// Returns: None 

// Parameters: Number of steps needs to be taken, initial position 

// Summary: Computes the joint angles according to the position 

Hf of the end effector(foot) by using the inverse kinematic 

H equations of the three link planar manipulator. The 

// algorithm for the path of the foot is described tn 

H Chapter IV of this thesis as “Stepping Forward Algorithm”. 
[ [nnn ennn nanan enn n nnn nn nnn nnn neem nnn nnn nnn nn nnn enna n-ne een one === ------ 2+ 


void step_forward(int number_of_steps,float X,float Y,float Z){ 


float 11 = 1.0f; 

float 12 = 1.0f; 

float cl_left,cl_ right; 
float theta] ,theta2; 
float k1_left,k2_ left; 
float k]_right,k2_ right; 


ileaexclen =" §.7321: 
float y_left= 0.0f: 
float x_right= 1.732f; 
float y_right= 0.0f; 


PaiiatAL HALF STEP 
foreloat Z— 32 > 2< 43 :z+= 0.5 * delta t) 
{ 
/* Goto sleep until next frame time. */ 
pfSync(); 
Shared->simTime = pfGetTime(); 


/* Main Body */ 
dcsO0->setRot(90.0, 90.0 , 0); 
dcs0->setTrans(X, Y,Z); 


i eticad.) +7 
dces7->setTrans(0.0 , 0.4 , 0.0); 


a Lorso 7 / 
des6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
des3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 


je eettioot 77 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
des9->setTrans(0 , -1.7, 0); 


/* Right foot */ 
des8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


NWP 


/* Leftlowerleg */ 
dcs4->setTrans(O , -1.7, 0); 


/* Leftupperleg */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/* Right upper arm */ 
dcsl 1->setScale(0.25f); 
dcs1 1->setTrans(-1.0, 1.7, 0); 


/* Right lower arm */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 


/* Right hand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0, 180.0); 


/ eit wpper arm) 7/ 
dcs14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 


/* Leftlower arm */ 
dcs15->setRot(0, 0, 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Lefthand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


TULL 
I Inverse Kinematics 

| 
TELL TL 
/1 SECOND STEP 

// RIGHT LEG STEP 


X meht = 095i *cos(64-=27 7) * DEG=TOLRAD)- 
(11 +12) * cos((64 - 2* z) * DEG_TO_RAD); 

y_right = 0.95f * cos((64 - 2* z) * DEG_TO_RAD )* 
(11 + 12) * sin((64 - 2* z) * DEG_TO_RAD); 


cl_nght= ((x_nght*x_night) + (y_nght*y_nght) 
= (1011) = C2412) 72 = 12). 
theta2 = (1 *acos(cl_night)); 


kl_right = 11 + (12 * cos(theta2)); 
Kzenght = l2 = sin(thetaz): 


thetal = atan(y_nght/x_right) - atan(k2_nght/k1_right); 
// Right Leg 


dcs2->setRot(0 ,(57.3 * thetal),0 ); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 
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// Left Arm 
dcs 14->setRot(O ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs15->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


CERES SEP 


x_left = cos((-64 + 2* z) * DEG_TO_RAD ) * (11 +12) * 
cos((-64 + 2* z) * DEG_TO_RAD); 
y_left = cos((-64 + 2* z) * DEG_TO_RAD ) * (11 +12) * 
sin((-64 + 2* z) * DEG_TO_RAD); 
cl_left= ((x_left*x_left) + (y_left*y_left) 
= (11*11) - (12*12))/Q * 11 *12); 
theta2 = (1 *acos(cl_left)); 


k1_left = 1] + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 
thetal = atan(y_left/x_left) - atan(k2_left/k 1_left); 


// Left leg 
dcs5->setRot(0 ,(57.3 * thetal), 0); 
dcs4->setRot(0 ,(57.3 * theta2), 0); 


// Right arm 
dcs11->setRot(O ,(0.7f *(57.3 * thetal) + 10.0f),0); 
dcs12->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f),0); 


// Torso rotation 
i(z< 31) 
dcs3->setRot((-(z - 21)/3.0),0 ,0); 
dcs6->setRot((-(z -21)/3.0),0 ,0); 
} 


else{ 
dcs3->setRot(((-20 + ((z- 21)))/3.0),0 0); 
dcs6->setRot(((-20 + ((z- 21)))/3.0),0 ,0); 


} 


dcsO->setTrans(X + (z - 32)* 0.1 ,Y, 
(Z + 0.2) + 0.08f * cos(2.0f * 3.14159f * (z - 41) / 21.0f)); 


Update View(); 
UpdateGUIQ; 
pfFrame(); 
}//End of second for 
for(i = 1 ; i< number_of_steps ; 1++){ 
fOl G@a—"i- 1) 42 el. 2 < (1-1 )*42 + 22 <7 += 0.5 * delta_t){ 
/* Go to sleep until next frame time. */ 


pfSync(); 
Shared->simTime=pfGetTime(); 


/* Main Body */ 


ae 


dcsO0->setRot(90.0, 90.0 , 0); 
dcs0->setTrans(X, Y,Z); 


/* Head */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


/*~ Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 


/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0, 180.0); 
dcs8->setTrans(O , -1.7, 0); 


/* Leftlowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 


7 ett uppene? 77 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/* Right upper arm */ 
dcsl1 1->setScale(0.25f); 
dcs11->setTrans(-1.0 , 1.7, 0); 


/* Right lower arm */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 


/* Right hand = */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(O, 0 , 180.0); 


/* Leftupperarm */ 
dcs14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 


/* Leftlowerarm */ 
dcs15->setRot(O, 0 , 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Lefthand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 
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TT 
// Inverse Kinematics 
THAT 
// FIRST STEP 
i LEFT LEG 
x_left = 0.95f * cos((22 - 2* (z- 42 *(i- 1))) * DEG_TO_RAD ) 
*(11 +12) * cos((22 - 2* (z- 42 *(i - 1))) * DEG_TO_RAD); 
y_left = 0.95f * cos((22 - 2* (z- 42 *(i- 1))) * DEG_TO_RAD ) 
*(11 + 12) * sin((22 - 2* (z- 42 *i- 1))) * DEG_TO_RAD); 


cl_left= ((x_left*x_left) + (y_left*y_left) - (11*11) 
ECs 12 (2 * |) *12); 

theta2 = (1 *acos(cl_left)); 

k1_left =11 + (12 * cos(theta2)); 

k2_ left = |Z * sin(thetaZ); 

thetal = atan(y_left/x_left) - atan(k2_left/k1_left); 


dces5->setRot(O ,(57.3 * thetal),0 ); 
dcs4->setRot(0 ,(57.3 * theta2) , 0); 


// Right Arm 
des] 1->setRot(O ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs12->setRot(O ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


// Right Leg 
X_right = cos((-22 + 2 * (z- 42 *(i- 1))) * DEG_TO_RAD ) 

* (11 +12) * cos((-22 + 2 * (z- 42 *(1 - 1))) * DEG_TO_RAD); 
y_right = cos((-22 + 2 * (z- 42 *(i- 1))) * DEG_TO_RAD ) 

* (1] +12) * sin((-22 + 2 * (z- 42 *(@i - 1))) * DEG_TO_RAD); 
cl_nght= ((x_nght*x_nght) + (y_nght*y_nght) 

y= (2712/2 * I) *12); 
theta2 = (1 *acos(cl_nght)); 


k]_nght = 11 + (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 
thetal = atan(y_right/x_night) - atan(k2_nght/k1_nght); 


dcs2->setRot(0 ,(57.3 * thetal), 0); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 


// Left Arm 
dcs]14->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs15->setRot (0 ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


// Torso rotation 
if((z- 42 *(7- 1)) < 11){ 
dcs3->setRot(((z- 42 *(i - 1))/3.0),0 .0); 
dcs6->setRot(((z- 42 *(i - 1))/3.0),0 .0); 
} 
else{ 
dcs3->setRot(((20 - (z- 42 *(i - 1)))/3.0),0 0); 
dces6->setRot(((20 - (z- 42 *(1 - 1)))/3.0),0 ,O); 
} 


dcsO->setTrans( X +1.0+2%*0.1 ,Y,Z2+02+0.08f 


13] 


*cos(2.0f #3 )4159t *z7/ 2101); 


UpdateView(); 

UpdateGUIQ; 

pfFrame(); 
} 
for (z= G-1)*42 +22 3z< (-1)*42 +43 ;z+=05 * delia yt), 

/* Go to sleep until next frame time. */ 

pfSync(); 

Shared->simTime = pfGetTime(); 


/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0); 
dcsQ->setTrans(X,Y,Z); 


/* Head */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


{=~ Torso) 77 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 


/* Leftfoot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0, 180.0); 
dcs9->setTrans(0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0, 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 


/* Leftupperleg */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/*Rightupper arm */ 
dcs1 1->setScale(0.25f); 
dcs11->setTrans(-1.0 , 1.7, 0); 


/* Right lower arm */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 


/* Righthand */ 


dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0 , 180.0); 
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/*Leftupperarm */ 
dcs14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 


/* Leftlowerarm */ 
dcs15->setRot(0, 0 , 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Lefthand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


LLL 
IT | Inverse Kinematics 
HAH 


TT 
1 SECOND STEP 
// RIGHT LEG STEP 


x_night = 0.95f * cos((64 - 2* (z- 42 *(1 - 1))) * DEG_TO_RAD ) 
* (11 + 12) * cos((64 - 2* (z- 42 *Gi - 1))) * DEG_TO_RAD),; 

y_right = 0.95f * cos((64 - 2* (z- 42 *(i - 1))) * DEG_TO_RAD ) 
* (11 +12) * sin((64 - 2* (z- 42 *(i - 1))) * DEG_TO_RAD); 


cl_nght= ((x_nght*x_nght) + (y_nght*y_nght) 
= ie) = (12*12))/(2 * 11 *12); 
theta2 = (1 *acos(cl_nght)); 


kl_nght = 1] + (12 * cos(theta2)); 
k2_nght = 12 * sin(theta2); 


thetal = atan(y_nght/x_nght) - atan(k2_nght/k1_night); 


// Right Leg 
dcs2->setRot(O ,(57.3 * thetal),0 ); 
desl->setRot(C ,(57.3 * theta2), 0); 


// Left ARM 
dcs14->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0); 
dcs15->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f),0); 


# LER) LEG STEP 
x_left = cos((-64 + 2* (z- 42 *(i - 1))) * DEG_TO_RAD ) 
* (11 + 12) * cos((-64 + 2* (z- 42 *G@ - 1))) * DEG_TO_RAD); 
y_left = cos((-64 + 2* (z- 42 *(i- 1))) * DEG_TO_RAD ) 
* (11 + 12) * sin((-64 + 2* (z- 42 *G - 1))) * DEG_TO_RAD), 


cl_left= ((x_left*x_left) + (y_left*y_left) - (11*11) 
lacey C2 |). *12): 
theta2 = (1 *acos(cl_left)); 


k1_left = 1] + (12 * cos(theta2)); 


k2_left = 12 * sin(theta2); 
thetal-= atan(y_left/x_left) - atan(k2_left/k1_left); 
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dcs5->setRot(O ,(57.3 * thetal), 0); 
dcs4->setRot(0 ,(57.3 * theta2), 0); 


// Right arm 
dcs11->setRot(O ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs12->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


// Torso rotation 
if((z- 42 *(—i - 1)) < 31){ 
dcs3->setRot((-(z- 42 *(i - 1) - 21)/3.0),0 ,0); 
dcs6->setRot((-(z- 42 *(i- 1) -21)/3.0),0 ,0):; 
} 
else{ 
dcs3->setRot(((-20 + ((z- 42 *(i - 1)- 21)))/3.0),0 ,0); 
dcs6->setRot(((-20 + ((z- 42 *(i - 1)- 21)))/3.0),0 ,O); 
} 


dcsO->setTrans(X + 1.04+2*0.1 ,Y,Z+0.2 + 0.08f 
= cos(Z.01 * 3:141591* z/ 2108); 

Update ViewQ; 

UpdateGUIQ(); 

pfFrame(); 


} 
\//Big FOR 


// LAST HALF STEP ///////// 
for Z= 2a = 3 deltact)¢ 


/* Go to sleep until next frame time. */ 
pfSync(); 
Shared->simTime=pfGetTime(); 


/* Main Body */ 


dcsO0->setRot(90.0, 90.0 , 0); 
dcesO->setTrans(X, Y,Z); 


/* Head #*/ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


/*= > Toro, 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 


/* Lett foot, 7/ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 


/* Right foot */ 


dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
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dcs8->setTrans(0 , -1.7, 0); 


/* Leftlowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 


(Ee upperies -*/ 
dcs5->setTrans(0.6, 0, 0); 


/* Rightlowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/* Rightupperarm = */ 
dcs]1->setScale(0.25f); 
dcs1]1->setTrans(-1.0 , 1.7, 0); 


/* Rightlowerarm */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 


/* Righthand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0, 180.0); 


/* Left upper arm */ 
dcs 14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 


/* Leftlower arm */ 
dcs15->setRot(0, 0, 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/*Lefthand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


TTL 
// Inverse Kinematics 
TTT 
// LEFT LEG 


x_left = 0.95f * cos((22 - 2* z) * DEG_TO_RAD ) *(1] +12) 
Pcos(z2-2 Zz) * DEG TO_RAD): 

y_left = 0.95f * cos((22 - 2* z) * DEG_TO_RAD ) *(11 +12) 
Psint(z2=27 Zz) * DEG TO_RAD); 


elmlen— (x leit*x_ left) + (y_left*y_left) 
Sey =12"*12))/(2 * 11. *12); 


theta2 = (1 *acos(cl_left)); 

k1_left =11 + (12 * cos(theta2)); 

k2_left = 12 * sin(theta2); 

thetal = atan(y_left/x_left) - atan(k2_left/k]_left); 


dcs5->setRot(O ,(57.3 * thetal),0 ); 
dcs4->setRot(O ,(57.3 * theta2) , 0); 


ESS 


// Right arm 
des11->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs12->setRot(0 ,(1.5f *(57.3 * thetal) - 10.06), 0); 


// RIGHT LEG 


x_right = cos((-22 + 2 * z) * DEG_TO_RAD ) * (11 + 12) 
* cos((-22 + 2 * z) * DEG_TO_RAD); 

y_right = cos((-22 + 2 * z) * DEG_TO_RAD ) * (11 + 12) 
* sin((-22 + 2 * z) * DEG_TO_RAD); 

cl_nght= ((x_right*x_nght) + (y_nght*y_night) - (11*11) 
C12" 12/2 = Al 12); 

theta2 = (1 *acos(cl_nght)); 


k]_right = 11 + (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 
thetal = atan(y_right/x_right) - atan(k2_night/k1_nght); 


dcs2->setRot(0 ,(57.3 * thetal), 0); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 


// Left arm 
dcs14->setRot(O ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs15->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f), 0); 
/1 TORSO rotation 
if(Z <1) 
dcs3->setRot((z/3.0),0 ,0); 
dcs6->setRot((z/3.0),0 ,0); 
} 
else{ 
dcs3->setRot(((20 - z)/3.0),0 ,0); 
dcs6->setRot(((20 - z)/3.0),0 ,0); 


} 
dcsO->setTrans(X + 1.0 + Gi-1) * 4.2+z* 0.1 
,Y,+Z2+0.2 + 0.08f *cos(2.0f * 3.14159f * z / 21.0f)); 


UpdateViewQ; 
UpdateGUIQ; 
pfFrame(); 

\// END of step_forward 


} 
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// Function: step_upward 

// Returns: None 

// Parameters: Number of steps needs to be taken, initial position 

// Summary: Computes the joint angles according to the position 

// of the end effector(foot) by using the inverse kinematic 

// equations of the three link planar manipulator. The 

// algorithm for the path of the foot is described in 

// Chapter IV of this thesis as “Stepping Upward Algorithm. 
// Height of each step is 0.267949 units. 

| |--------------- 27-2222 nner nennnn 


void step_upward(int number_of_steps,float X.float Y float Z){ 


float 11 = 1.0f; 

neat l2 = 10t- 

float cl_left,cl_nght; 
float thetal ,theta2; 
float k]_left,k2_left; 
float k]_nght,k2_nght; 


float x_left= 1.732f; 
float y_left= 0.0f; 
float x_nght= 1.732f; 
float y_nght = 0.0f; 
float Z; 


TTT 
// FIRST HALF STEP 
THAT 
fomeZ = 08 7 — 2207 += 0.3 * delta_t) 
{ 


/* Go to sleep until next frame time. */ 
pfSync(); 
Shared->simTime=pfGetTime(); 


/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0.0); 
dcsO->setTrans(X,Y,Z); 


/* Head */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
des3->setTrans(0.3 , 2.1 , 0.005); 


/-Meti foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 


eit! 


dcs9->setTrans(0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 


/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg = */ 
dcsl->setTrans(0 _ , -1.7, 0); 


/* Right upper arm */ 
dcs11->setScale(0.25f); 


/* Right lower arm */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 


/* Right hand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(O, 0 , 180.0); 


/* ett upper ami 7 
dcs14->setScale(0.25f); 
//dcs14->setTrans(1.6 , 1.8, Q); 


/* Left lower arm */ 
dcs15->setRot(0, 0 , 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Lefthand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


LLL 
// Inverse Kinematics 
TUT 
// FIRST STEP 
f/f LEFT LEG 
x_left = 0.95f * cos((22 - 2* z - 10) * DEG_TO_RAD ) *(11 + 12) 
* cos((22 - 2* z - 10) * DEG_TO_RAD); 
y_left = 0.95f * cos((22 - 2* z - 10) * DEG_TO_RAD ) *(I1 + 12) 
* sin((22 - 2* z - 10) * DEG_TO_RAD); 


cl_left= ((x_left*x_left) + (y_left*y_left) - (11*11) 
12712) (2 tie): 


theta2 = (1 *acos(cl_left)); 
k1_left = 11 + (12 * cos(theta2)); 


k2_left = 12 * sin(theta2); 
thetal = atan(y_left/x_left) - atan(k2_left/k1_left); 
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// 


dcs5->setRot(0 ,(57.3 * thetal),0 ); 
dcs4->setRot(0 ,(57.3 * theta2) , 0); 


// Right arm 

dcs11->setTrans(-1.0 , 1.7, 0.5); 

dcs11->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs12->setRot(O ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


RIGHT LEG 

X_night = cos((-22 + 2 * z - 10) * DEG_TO_RAD ) * (11 +12) 
* cos((-22 + 2 * z - 10) * DEG_TO_RAD); 

y_night = cos((-22 + 2 * z - 10) * DEG_TO_RAD ) * (11 +12) 
* sin((-22 +2 * z - 10) * DEG_TO_RAD); 

cl_nght= ((x_nght*x_nght) + (y_nght*y_nght) - (11*1]) 
=(12712))/@ * 11*12): 

theta2 = (1 *acos(cl_nght)); 


kl_nght = 11 + (12 * cos(theta2)); 
k2_night = 12 * sin(theta2); 
thetal = atan(y_nght/x_nght) - atan(k2_nght/k1_nght); 


dcs2->setRot(0 ,(57.3 * thetal), 0); 

dcsl->setRot(0 ,(57.3 * theta2), 0); 

// Left arm 

dcs14->setTrans(1.6 , 1.8, 0.5); 

dcs14->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 

dcs15->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f), 0); 

// TORSO rotation 

nz =) ( 
des3->set Irans(0.3, 2.1) , 0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot((z/3.0),7.0,0); 
dcs6->setRot((z/3.0),7.0,0); 

} 

else{ 
dcs3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot(((20 - z)/3.0) ,7.0,0); 
dcs6->setRot(((20 - z)/3.0) ,7.0,0); 

} 

Z= 2z+21; 

dcsQ->setTrans( X + (z - 32) * 0.1 -0.4, Y , 
Ze Wo5068 4) * 0,08f *cos(2.0f * 3.141 59f * z/ 21.0f) */0 
+ 1 * 0.267949 * 3.4 *2 +((1] +12) 
-(11 +12) * cos(( -22 + 2 * z - 10) * DEG_TO_RAD))); 

Z= 7-21. 


UpdateView(); 
UpdateGUI(); 
pfFrame(); 


\//END FOR 


Loe, 


for(i= 1 ;i<number_of_steps ; 1++){ 


for (z= (i-1)*42 + 22.0 ;z < (i-1)*42 + 43.0 ; z += 0.35 * delta_t){ 
/* Go to sleep until next frame time. */ 
pfSync(); 
Shared->simTime = pfGetTime(); 


/* Main Body */ 
dcsOQ->setRot(90.0, 90.0 , 0.0); 
dcsO->setTrans(X, Y,Z); 


/* Head */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


> Torso 8 77 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 


dcs3-SsetTrans(0.3 ; 2.1 , 0.005): 


/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0, 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 


/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/* Right upper arm */ 
dcs11->setScale(0.25f); 


/* Right lower arm */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 


/* Right hand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0 , 180.0); 


/* Lett upper armen, 
dcs]4->setScale(0.25f); 


/* Left lower arm */ 
dcs15->setRot(0, 0 , 180.0); 
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dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Lefthand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


TTT 
// Inverse Kinematics 
HHH 


TILT TLELT 
/! SECOND STEP 
7 RIGHT LEG STEP 


x_right = 0.95f * cos((64 - 2* (z- 42 *(i- 1)) - 10) * DEG_TO_RAD) 
* (11 +12) * cos((64 - 2* (z- 42 *(i- 1)) - 10) 
* DEG_TO_RAD); 

y_right = 0.95f * cos((64 - 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD) 
* (11 + 12) * sin((64 - 2* (z- 42 *(@i - 1)) - 10) 
* DEG_TO_RAD); 

cl_nght= ((x_nght*x_nght) + (y_nght*y_nght) - (11*11) 
sl ye * N12): 

theta2 = (1 *acos(cl_nght)); 


kl_nght = 11 + (12 * cos(theta2)); 
k2_night = 12 * sin(theta2); 
thetal = atan(y_nght/x_nght) - atan(k2_nght/k1_right); 


// Right LEG 
dcs2->setRot(0 ,(57.3 * thetal ),0 ); 
dcs1->setRot(0 ,(57.3 * theta2), 0); 


// Left arm 

dcs14->setTrans(1.6 , 1.8, 0.5); 

dcs14->setRot(0O ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs15->setRot(O ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


i SLEFT LEG STEP 
x_left = cos((-64 + 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD ) 
(ile 12) 
* cos((-64 + 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD); 
y_left = cos((-64 + 2* (z- 42 *( - 1)) - 10) * DEG_TO_RAD ) 
* (11 + 12) 
* sin((-64 + 2* (z- 42 *(1- 1)) - 10) * DEG_TO_RAD); 


cl_left= ((x_left*x_left) + (y_left*y_left) - (11*11) 
(12 2 ))7(2-- 11 *12); 

ineta? — (lr acos(c). left)); 

kl_left =11 + (12 * cos(theta2)); 

k2_left = 12 * sin(theta2); 

thetal = atan(y_left/x_left) - atan(k2_leftv/k1_left): 


dcs5->setRot(0 ,(57.3 * thetal), 0); 
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dcs4->setRot(0 ,(57.3 * theta2), 0); 


// Right arm 

dcs11->setTrans(-1.0 , 1.7, 0.5); 

dcsl1->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs12->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


// TORSO rotation 

if((zZ-42 40-1) st 
dcs3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot((-(z- 42 *(i - 1) - 21)/3.0) ,7.0,0); 
dcs6->setRot((-(z- 42 *(i - 1) -21)/3.0) ,7.0,0); 

} 


else{ 

dcs3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot(((-20 + ((z- 42 *(i - 1)- 21)))V/3.0) ,7.0,0); 
dcs6->setRot(((-20 + ((z- 42 *(i - 1)- 21)))/3.0) ,7.0,0); 


Za=Z-216 
dcsO->setTrans( X +0.5+2z%*0.1 ,Y,Z 

+ 1 * 0.267949 * 3.4 *2 - ((11 + 12) - (11 + 12) 

* cos(( 64 - 2 * (z- 42 *(i- 1)) )* DEG_TO_RAD))); 
Z = 7 


Update View(); 
UpdateGUI(); 
pfFrame(); 

} 


for (z = (i-1)*42 + 1.0 5 z < (i-1)*42 + 22.0 ; z += 0.35 * delta_t){ 


/* Goto sleep until next frame time. */ 
pfSync(); 
Shared->simTime=pfGetTime(); 


/* Main Body */ 


dcs0O->setRot(90.0, 90.0 ,0); 
dcsO->setTrans(X, Y ,Z); 


/* Head */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


[> Torso +) 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 
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/* Leftfoot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(O , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0, 180.0); 
dcs8->setTrans(0O , -1.7, 0); 


/* Leftlowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 


7 Letrupperies */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(O , -1.7, 0); 


/* Right upper arm */ 
des] 1->setScale(0.25f); 


/* Right lower arm = */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 


/*Righthand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
des] 3->setRot(0, 0 , 180.0); 


/* Leftupper arm */ 
dcsl4->setScale(0.25f); 


/* Leftlower arm */ 
dcs15->setRot(O, 0 , 180.0): 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Left hand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


TTT 


Inverse Kinematics 


TTT 
FIRST STEP 
PEER TLEG 


x_left = 0.95f * cos((22 - 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD ) 
*(11 + 12) * cos((22 - 2* (z- 42 *(i- 1)) - 10) *DEG_TO_RAD); 
y_left = 0.95f * cos((22 - 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD ) 
*(1] +12) * sin((22 - 2* (z- 42 *(i- 1)) - 10) *DEG_TO_RAD); 
cl_left= ((x_left*x_left) + (y_left*y_left) - (11*11) 
- (12*12))/(2 * 11 *12); 


theta2 = (1 *acos(cl_left)); 


k1_left = 11 + (12 * cos(theta2)); 
kZslett = 12 * sin(theta2); 
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thetal = atan(y_left/x_left) - atan(k2_left/k1_left); 


dcs5->setRot(0 ,(57.3 * thetal),0 ); 
dcs4->setRot(0 ,(57.3 * theta2) , 0); 


// Right arm 

des11->setTrans(-1.0 , 1.7, 0.5); 

dcs11->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs12->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


// RIGHT LEG 

x_night = cos((-22 + 2 * (z- 42 *(1- 1)) - 10) * DEG_TO_RAD ) 
* (11 + 12) * cos((-22 + 2 * (z- 42 *(i - 1)) - 10) 
* DEG_TO_RAD); 

y_right = cos((-22 + 2 * (z- 42 *(1- 1)) - 10) * DEG_TO_RAD ) 
* (11 + 12) * sin((-22 + 2 * (z- 42 *G7 - 1)) - 10) 
* DEG_TO_RAD); 

cl_right= ((x_right*x_night) + (y_nght*y_nght) - (11*1]) 
(L212 = eee 

theta2 = (1 *acos(cl_nght)); 


k1_right = 11 + (12 * cos(theta2)); 
k2_night = 12 * sin(theta2); 
thetal = atan(y_right/x_nght) - atan(k2_nght/k]_right); 


dcs2->setRot(0 ,(57.3 * thetal), 0); 
dcesl->setRot(O ,(S7.3 * theta2), 0); 
// Left arm 
dcs14->setTrans(1.6 , 1.8, 0.5); 
des 14->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
des 15->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f), 0); 
// TORSO rotation 
if((z- 42 *1- 1))< 11){ 
dces3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot(((z- 42 *(i - 1))/3.0) ,7.0 ,0); 
dcs6->setRot(((z- 42 *(1 - 1))/3.0) ,7.0 ,0); 
} 
else { 
dcs3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot(((20 - (z- 42 *(1 - 1)))/3.0) ,7.0 ,0); 
dcs6->setRot(((20 - (z- 42 *(1 - 1)))/3.0) ,7.0 ,0); 
} 
P= 2to 
dcsO->setTrans(X +0.5+2z%*0.1 ,Y ,Z-0.105327 
+1 * 0.267949 * 3.4*2 + ((11 +12) -(1] +12) 
* cos(( -22 + 2 * (z- 42 *(1- 1)) - 10) 
* DEG_TO_RAD))); 
Z= 7-2). 
Update View(); 
UpdateGUIQ(); 
pfFrame(); 
}//END FOR 
}//Big FOR- 
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TATA ATT LT 
// LAST HALF 
TTT TTT 


POE 2200 2 55.) 2 += (0.35 * delta t){ 
/* Go to sleep until next frame time. */ 
pfSync(); 

Shared->simTime = pfGetTime(); 


/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0.0): 
dcsO->setTrans(X,Y,Z); 


[= tlead */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


= Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 


y* Lettfoot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(O , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 


/* Leftupperleg */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/* Right upper arm */ 
des] 1->setScale(0.25f); 


/* Right lower arm */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 


/* Righthand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0 , 180.0); 


/* Left upper arm */ 
des 14->setScale(0.25f); 
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/* Leftlower arm */ 
dcs15->setRot(0, 0 , 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Lefthand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


TUITE 


HI 


Inverse Kinematics 


TTT 


ITT 
// SECOND STEP 
// RIGHT LEG STEP 


// 


x_right = 0.95f * cos((64 - 2* z - 10) * DEG_TO_RAD )* (11 +12) 
* cos((64 - 2* z - 10) * DEG_TO_RAD); 

y_nght = 0.95f * cos((64 - 2* z - 10) * DEG_TO_RAD )* (11 +12) 
* sin((64 - 2* z - 10) * DEG_TO_RAD); 


cl_nght= ((x_nght*x_nght) + (y_nght*y_nght) - (11*1]) 
=(12*12))/(2 “Ti #12): 
theta2 = (1 *acos(cl_right)); 


k]_night = 11 + (12 * cos(theta2)); 
k2_night = 12 * sin(theta2); 


thetal = atan(y_nght/x_nght) - atan(k2_nght/k]_nght); 


// Right LEG 
dcs2->setRot(0 ,(57.3 * thetal),0 ); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 


// Left arm 

dcs14->setTrans(1.6 , 1.8, 0.5); 

dcs14->setRot(O ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs15->setRot(O ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


LEFT LEG STEP 

x_left = cos((-64 + 2* z - 10) * DEG_TO_RAD ) * (11 +12) 
* cos((-64 + 2* z - 10) * DEG_TO_RAD),; 

y_left = cos((-64 + 2* z - 10) * DEG_TO_RAD ) * (11 + 12) 
* sin((-64 + 2* z - 10) * DEG_TO_RAD); 


cl_left= (x sleit*x ett) + (yleht*)_ let) =i 


=C2412) 2 ale 12). 
theta2 = (1) acos(cl alent): 


kl_left = 11 + (12 * cos(theta2)); 
k2 left 12 * sin(theta2): 
thetal = atan(y_left/x_left) - atan(k2_left/k1_left); 


dcs5->setRot(0 ,(57.3 * thetal), 0); 
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dcs4->setRot(0 ,(57.3 * theta2), 0); 
// Right arm 


dcsl1->setTrans(-1.0 , 1.7, 0.5); 
dcsl1->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcs12->setRot(O ,(1.5f *(57.3 * thetal) - 10.0f), 0); 


// Torso rotation 
if(z < 31){ 
dcs3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot((-z - 21)/3.0 +13.75,7.0,0); 
dcs6->setRot((-z -21)/3.0 +13.75,7.0,0); 
} 
else{ 
dcs3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot((-20+ z- 21)/3.0 ,7.0,0); 
dcs6->setRot((-20 + z- 21)/3.0 ,7.0,0); 
} 
B= 7-21: 
dcsO0->setTrans( X + 0.5 +(i-1) * 4.2+2*0.1 ,Y,Z 
+ number_of_steps * 0.267949 * 3.4 *2 - (11 +12) 
- (11 +12) * cos(( 64 - 2 * z) * DEG_TO_RAD))); 


Z=7+21: 


Update View(); 
UpdateGUI(); 
pfFrame(); 
}//End of for 

}//fEnd of step_upward 
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// 2Buncuon: jump 

// Returns: None 

// Parameters: Initial position 

// Summary: Translate the whole body first straight upward, 

// secondly along a semi circle path, then straight down 

If and straight up to an upright position. While translating 
// the body appropriate joint angles are applied. 
ee 

void 


jump(float X,float Y,float Z){ 


float I=s1.0f; 

float 12= 1.01; 

float cl_left,cl_nght; 
float thetal ,theta2; 
float k]_left,k2_left; 
float kl_nght,k2_right; 


float x2 lett =a 321. 
float y_left= 0.0f; 
float x_right = 1.732f; 
float y_nght = 0.0f; 


for (float |= 1) jp 10 je —deliant){ 
/* Go to sleep until next frame time. */ 
pfSync(); 
Shared->simTime = pfGetTime(); 


/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0); 
dcsO->setTrans(X+j*0.8/100.0, YZ - j * 1.5/100.0); 


/* Head */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


/* “horso. 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dces3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 
dcs3->setRot(0, 10.0*j/100.0 , 0.0); 
dcs6->setRot(0, 10.0*j/100.0 , 0.0); 


x lett —se3> 0 4 100-0: 
y_ leit = 0:0; 


cl_left="((xnlett*xcleiy ety lett ylety) — (i 1) 
=(12*12)jA2 * 11 12), 


theta2 = (1 *acos(cl1_left)); 


k1_left = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 


148 


thetal = atan(y_left/x_left) - atan(k2_left/k1_left); 


dcs5->setRot(O ,(57.3 * thetal),0O ); 
dcs4->setRot(0 ,(57.3 * theta2) , 0); 
dcs2->setRot(0 ,(57.3 * thetal),0O ); 
dcs1->setRot(0 ,(S7.3 * theta2) , 0); 


/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


i eet lowerleo —*/ 
dcs4->setTrans(O , -1.7, 0); 


(= Cet uppernes  */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/* Rightupper arm */ 
dcs11->setScale(0.25f); 
dcs]l1->setTrans(-1.0 , 1.7, 0); 
dcs11->setRot(0.0,-j/5.0 ,0.0); 


/* Right lower arm */ 
dcs]12->setTrans(0.4 , -4.6, 0.0); 
dcs12->setRot(0.0 ,-j/5.0,0.0); 


/* Righthand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0 , 180.0); 


/* Left upper arm */ 
dcs14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 
dcs14->setRot(0.0 ,-j/5.0,0.0); 


/* Left lower arm “*/ 
des15->setRot(0,-j/5.0, 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Left hand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


UpdateView(); 
UpdateGUI(Q); 
pfFrame(); 


~ 
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for (float k= 1 :k< 101 >k +=5:0- delta_1){ 
/* Go to sleep until next frame time. */ 
pfSync(); 
Shared->simTime = pfGetTime(); 


x_left = 0.95 + 0.5 * k /100.0; 
y_left = 0.0; 


/* Main Body */ 
dcs0->setRot(90.0 , 90.0 + 15.0*k/100.0 , 0); 
dcsO->setTrans(X+ 100.0*0.8/100.0 + (x_left* 1.7 
* sin(15.0*k* DEG_TO_RAD/100.0)), Y , 
Z- 100.0 * 1.5/100.0 + 0.95 * k /100.0 - x_left* 1.7 
*(1.0 - cos(15.0*k* DEG_TO_RAD/100.0))); 


/* ~~ Head > */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


(7 lOTsOn, a. 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 
dcs3->setRot(0, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 


cl_left= ((x_left*x_left) + (y_left* y_left) - (11*11) 
=12*1 2) ales l2)- 


theta2 = (1 *acos(cl_left)); 


k1]_left = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 
theta] = atan(y_left/x_left) - atan(k2_left/k1_left); 


dcs5->setRot(0 ,(57.3 * thetal) ,0); 
dcs4->setRot(0 ,(57.3 * theta2) ,0); 
dcs2->setRot(0 ,(57.3 * thetal) ,0); 
dcsl->setRot(0 ,(57.3 * theta2) ,O); 


(Lett To0twaa: 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dces8->setTrans(0 , -1.7, 0); 


/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 


/* Left upperies 
dcs5->setTrans(0.6, 0, 0); 
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/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/* Right upper arm */ 
dcs11->setScale(0.25f); 
dcs11->setTrans(-1.0 , 1.7, 0); 
des11->setRot(0.0,-j/5.0 ,0.0); 


/* Right lower arm = */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 
dcs12->setRot(0.0 ,-j/5.0,0.0); 


/*Righthand */ 
des 13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0, 180.0); 


/* Leftupper arm */ 
dcs14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 
dcs14->setRot(0.0 ,-j/5.0,0.0); 


/* Leftlower arm */ 
dcs15->setRot(0,-j/5.0, 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Lefthand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


Update View(); 
UpdateGUI(); 
pfFrame(); 

} 

for (float 1= 1 ;1< 101 ;1+=delta_t){ 
/* Go to sleep until next frame time. */ 
pfSync(); 
Shared->simTime = pfGetTime(); 


Mette =ola 5: 
y_left = 0.0; 


/* MainBody */ 
dcsO->setRot(90.0 , 90.0+ 360.0*1/100.0 , 0); 


dcsO->setTrans(3.0 + 3.0*cos((180.0 - 180.0* 1/100.0)*DEG_TO_RAD)+ X 
+ 100.0*0.8/100.0 + (x_left* 1.7 
* sin(15.0*100.0*DEG_TO_RAD/100.0)),Y , 
3.0*sin((180.0 - 180.0* 1/100.0)*DEG_TO_RAD) 
+ Z- 100.0 * 1.5/100.0+ 0.95 * 100.0/100.0 - x_left 
* 1.7*(1.0 - cos(15.0* 100.0* DEG_TO_RAD/100.0))); 


/* SHead */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 
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/* - Torso. */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 
dcs3->setRot(0, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 


cl left= ((xcleit*x_leit) + (y lere yatet= al-)) 
=(12-12) (2 Wyld): 


theta2 = (1 *acos(cl_left)); 


k1_left = 11 + (2 * cos(theta2)); 
k2_left =12 * sin(theta2): 
thetal = atan(y_left/x_left) - atan(k2_left/k1_left): 


dcs5->setRot(0 (57.3 * thetal) ,0); 
dces4->setRot(O ,(57.3 * theta2) ,0); 
dcs2->setRot(O ,(57.3 * thetal) ,0); 
dcsl->setRot(O ,(57.3 * theta2) ,0); 


/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(O0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dces&->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


/* Left lowerleg */ 
dcs4->setTrans(O0_ , -1.7, 0); 


/* Leftupperleg */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(O , -1.7, 0); 


/* Right upper arm */ 
dces11->setScale(0.25f); 
dcs]1->setTrans(-1.0 , 1.7, 0); 
dcs11->setRot(0.0,-100.0/5.0 ,0.0); 


/* Right lower arm */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 
dces12->setRot(0.0 ,-100.0/5.0,0.0); 


/* Right hand */ 


dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0 , 180.0); 
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/* Left upperarm */ 
dcs14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 
dcs14->setRot(0.0 ,-100.0/5.0,0.0); 


/* Leftlowerarm */ 
dcs15->setRot(0,-100.0/5.0, 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Left hand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


UpdateView(); 
UpdateGUI(); 
pfFrame(); 

} 

for (float m=1 ;m< 101 ;m+=3.0 * delta_t){ 
/* Goto sleep until] next frame time. */ 
pfSync(); 
Shared->simTime = pfGetTime(); 


xeleie= 1-45: 
y_left = 0.0: 


/* Main Body */ 
dcs0->setRot(90.0 , 90.0+ 360.0*100.0/100.0 , 0); 
dcsO->setTrans(3.0 + 3.0*cos((180.0 - 180.0* 100.0/100.0)*DEG_TO_RAD)+X 
+ 100.0*0.8/100.0 + (x_left* 1.7 
* sin(15.0* 100.0* DEG_TO_RAD/100.0)),Y , 
3.0*sin((180.0 - 180.0* 100.0/100.0)*DEG_TO_RAD) 
+ Z- 100.0 * 1.5/100.0 + 0.95 * 100.0/100.0 - x_left 
* 1.7*(1.0 - cos(15.0* 100.0*DEG_TO_RAD/100.0)) 
- 9.01*m/100.0); 


/* Head */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 
dcs3->setRot(0, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 


cl_left= ((x_left*x_left) + (y_left* y_left) - (11*11) 
ee 12))/(2.* 1) * 12); 


theta2 = (1 *acos(cl_left)); 

k1_left = 11 + (12 * cos(theta2)); 

k2alett = 12 = sin(theta2); 

thetal = atan(y_left/x_left) - atan(k2_left/k1_left); 


dcs5->setRot(O ,(57.3 * thetal) ,O); 
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dcs4->setRot(0 ,(57.3 * theta2) ,0); 
dcs2->setRot(0 ,(57.3 * thetal) ,0); 
dcsl->setRot(0 ,(57.3 * theta2) ,O); 


/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


/* Wett lowerles We, 
dcs4->setTrans(0 , -1.7, 0); 


/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/* Right upper arm */ 
dcs11->setScale(0.25f); 
dcs11->setTrans(-1.0 , 1.7, 0); 
dcs] 1->setRot(0.0,-100.0/5.0 ,0.0); 


/* Right lower arm */ 
dcs]2->setTrans(0.4 , -4.6, 0.0); 
dcs]2->setRot(0.0 ,-100.0/5.0,0.0); 


/* Right hand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0 , 180.0); 


7 ett upper ati 
dcs14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 
dcs14->setRot(0.0 ,-100.0/5.0,0.0); 


/* Left lower arm */ 
dcs15->setRot(0,-100.0/5.0, 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/*Eetthand = 77 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


Update View(); 
UpdateGUI(); 
pfFrame(); 


} 
for float n= il on< 101 n= 5.0 * deliazt)| 
/* Go to sleep until next frame time. */ 


pfSync(); 
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Shared->simTime = pfGetTime(Q); 


x_left = 1.45 + 0.5 *n/100.0; 
valere— 0.0; 


/* Main Body */ 
dcs0->setRot(90.0 , 90.0+ 360.0* 100.0/100.0 , 0); 


des0->setTrans(3.0 + 3.0*cos((180.0 - 180.0* 100.0/100.0)*DEG_TO_RAD)+X 
+ 100.0*0.8/100.0 + (1.45* 1.7 
* sin(15.0* 100.0* DEG_TO_RAD/100.0)),Y , 
3.0*sin((180.0 - 180.0* 100.0/100.0)*DEG_TO_RAD) 
+ Z- 100.0 * 1.5/100.0 + 0.95 * 100.0/100.0 - 1.45 
* 1.7*(1.0 - cos(15.0* 100.0* DEG_TO_RAD/100.0)) 
-9.01*100.0/100.0+ n* 1.8/100.0); 


j= Head “*/ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 


i forse” =*/ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f): 
dcs3->setTrans(0.3 , 2.1 , 0.005); 
dcs3->setRot(O, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 


Clulett=<(xslett=x_left) + (y_left* y_left) - (11*11) 
Zatz = 1) *12): 


thetaz)= (1 =acos(cl_left)); 


kl] _left = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 
thetal = atan(y_left/x_left) - atan(k2_left/k1_left); 


dcs5->setRot(O ,(57.3 * thetal) ,0); 
dcs4->setRot(0 ,(57.3 * theta2) ,0); 
dcs2->setRot(O ,(57.3 * thetal) ,0); 
dcsl->setRot(O ,(57.3 * theta2) ,0); 


fei toot. 7) 
dces9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


/* Leftlowerleg */ 
dcs4->setTrans(O , -1.7, 0); 


7 -wettupperles ~/ 
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dcs5->setTrans(0.6, 0, Q); 


/* Right lowerleg */ 
dcsl->setTrans(0 ,-1.7, 0); 


/* Right upper arm */ 
dcs11->setScale(0.25f); 

desl 1->setTrans(-1.0 , 1.7, 0); 
dcs11->setRot(0.0,-100.0/5.0 ,0.0):; 


/* Right lower arm */ 
dcs12->setTrans(0.4 , -4.6, 0.0); 
dcs12->setRot(0.0 ,-100.0/5.0,0.0); 


/* Right hand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0, 180.0); 


/* Left upper arm */ 
dcs14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 

dcs 14->setRot(0.0 ,- 100.0/5.0,0.0); 


/* Leftlowerarm */ 
dcs15->setRot(0,-100.0/5.0, 180.0); 
dcs15->setTrans(-0.5 , -4.6, 0.0); 


/* Left hand */ 
dcs16->setTrans(-0.2 , -4.6, 0.0); 


Update View(); 
UpdateGUI(); 
pfFrame(); 

} 

for (float o= 1 ;0 < 101 ; 0 += 2.0 * delta_t)( 
/* Go to sleep until next frame time. */ 
pfSyncQ); 
Shared->simTime = pfGetTime(); 


x_left = 1.45 + 0.5 *100.0/100.0; 
y_left = 0.0; 


/* Main Body */ 
dcsO->setRot(90.0 , 90.0+ 360.0*100.0/100.0 , 0); 


dcs0->setTrans(3.0 + 3.0*cos((180.0 - 180.0* 100.0/100.0)* DEG_TO_RAD)+X 
+ 100.0*0.8/100.0 + (1.45* 1.7 
* sin(15.0*100.0*DEG_TO_RAD/100.0)),Y , 3.0 
*sin((180.0 - 180.0* 100.0/100.0)*DEG_TO_RAD) 
+ Z- 100.0 * 1.5/100.0 + 0.95 * 100.0/100.0 - 1.45* 1.7 
* (1.0 - cos(15.0*100.0* DEG_TO_RAD/100.0)) 
-9.01*100.0/100.0 + n* 1.8/100.0); 


/* Head */ 
dcs7->setTrans(0.0 , 0.4 , 0.0); 
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i Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 
dcs3->setRot(0, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 


cl_left= ((x_left*x_left) + (y_left*y_left) - (11*11) 
=I2"12) (22 112): 


theta2 = (1 *acos(cl_left)); 


k1_left = 11 + (12 * cos(theta2)); 
kwen =l2 * sintthetaz): 
thetal = atan(y_left/x_left) - atan(k2_left/k1_left); 


dcs5->setRot(0 ,(57.3 * thetal) ,0): 
dcs4->setRot(0O ,(57.3 * theta2) ,0); 
dcs2->setRot(O ,(57.3 * thetal) ,0); 
dcsl->setRot(O ,(57.3 * theta2) ,0); 


mo leeit foot, */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 1 80.0); 
dcs9->setTrans(O0 , -1.7, 0); 


/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 


/ Cett lowerles 7 
dcs4->setTrans(0 , -1.7, 0); 


/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 


/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 


/* Right upper arm */ 

dcs] 1->setScale(0.25f); 
dcs11->setTrans(-1.0 , 1.7, 0); 
dcs1 1->setRot(0.0,-100.0/5.0 ,0.0); 


/* Right lower arm */ 
dcs1]2->setTrans(0.4 , -4.6, 0.0); 
dces12->setRot(0.0 ,-100.0/5.0,0.0); 
/* Right hand */ 
dcs13->setTrans(0.0 , -4.2, 0.0); 
dcs13->setRot(0, 0 , 180.0); 


/* Lettupperarm */ 
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dcs14->setScale(0.25f); 
dcs14->setTrans(1.6 , 1.8, 0); 
des 14->setRot(0.0 ,-100.0/5.0,0.0); 


/* Left lower arm */ 


dcs15->setRot(0,-100.0/5.0, 180.0); 


dces15->setTrans(-0.5 , -4.6, 0.0); 


/* Lefthand */ 
des16->setTrans(-0.2 , -4.6, 0.0); 


UpdateView(); 
UpdateGUI(); 
pfFrame(); 
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